83 lines
1.8 KiB
C++
83 lines
1.8 KiB
C++
#include "headers.h"
|
|
#include "AS5600.h"
|
|
|
|
AS5600 as5600_0(&Wire1);
|
|
AS5600 as5600_1(&Wire2);
|
|
|
|
EncoderOdom odom;
|
|
|
|
void zeroOdom(){
|
|
|
|
odom.left = 0.0;
|
|
odom.right = 0.0;
|
|
odom.x = 0.0;
|
|
odom.y = 0.0;
|
|
odom.angle = 0.0;
|
|
|
|
wire2();
|
|
as5600_0.resetCumulativePosition(0);
|
|
wire1();
|
|
as5600_1.resetCumulativePosition(0);
|
|
}
|
|
|
|
|
|
void initEncoder(){
|
|
odom.left = 0.0;
|
|
odom.right = 0.0;
|
|
odom.x = 0.0;
|
|
odom.y = 0.0;
|
|
odom.angle = 0.0;
|
|
|
|
wire2();
|
|
as5600_0.begin();
|
|
Serial.print("Connect device 0: ");
|
|
Serial.println(as5600_0.isConnected() ? "true" : "false");
|
|
as5600_0.resetCumulativePosition(0);
|
|
|
|
wire1();
|
|
as5600_1.begin();
|
|
Serial.print("Connect device 1: ");
|
|
Serial.println(as5600_1.isConnected() ? "true" : "false");
|
|
as5600_1.resetCumulativePosition(0);
|
|
}
|
|
|
|
void updateEncoder(){
|
|
#define WHEEL_CIRCUM (2*PI*4.0625)
|
|
#define WHEEL_DISTANCE (7 + 12.0/16.0)
|
|
|
|
wire2();
|
|
int rawL = as5600_0.getCumulativePosition();
|
|
wire1();
|
|
int rawR = as5600_1.getCumulativePosition();
|
|
|
|
float left = rawL/4096.0 * -WHEEL_CIRCUM;
|
|
float right = rawR/4096.0 * WHEEL_CIRCUM;
|
|
|
|
float d_left = left-odom.left;
|
|
float d_right= right-odom.right;
|
|
|
|
odom.left = left;
|
|
odom.right = right;
|
|
|
|
float displacement = (d_left + d_right)/2;
|
|
float oldAng = odom.angle;
|
|
odom.angle += (d_left-d_right)/(2*WHEEL_DISTANCE);
|
|
float ang = (odom.angle+oldAng)/2;
|
|
|
|
float dispx = odom.x-desiredPos.x;
|
|
float dispy = odom.y-desiredPos.y;
|
|
posInput = sqrt(dispx*dispx+dispy*dispy);
|
|
|
|
desiredYaw = atan2(dispy, dispx)*180/PI;
|
|
if(abs(fmod(desiredYaw-currentYaw, 180.0))<90){
|
|
desiredYaw = fmod(desiredYaw+180.0, 360.0);
|
|
if(desiredYaw>180)desiredYaw-=180.0;
|
|
|
|
posInput = -posInput;
|
|
}
|
|
// Serial.println(desiredYaw);
|
|
|
|
odom.x += (float)(cos(ang)*displacement);
|
|
odom.y += (float)(sin(ang)*displacement);
|
|
}
|