49 lines
925 B
C++
49 lines
925 B
C++
|
#include "Navigation.h"
|
||
|
|
#include "hardware.h"
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
|
||
|
|
|
||
|
|
Navigation::Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3) {
|
||
|
|
motors[0] = m1;
|
||
|
|
motorPos[0] = p1;
|
||
|
|
motors[1] = m2;
|
||
|
|
motorPos[1] = p2;
|
||
|
|
motors[2] = m3;
|
||
|
|
motorPos[2] = p3;
|
||
|
|
|
||
|
|
speed = direction = spin = 0.0;
|
||
|
|
|
||
|
|
update();
|
||
|
|
}
|
||
|
|
|
||
|
|
void Navigation::update() {
|
||
|
|
float s1, s2, s3;
|
||
|
|
|
||
|
|
s1 = s2 = s3 = spin;
|
||
|
|
|
||
|
|
s1 += speed*sin((direction-motorPos[0])*M_PI/180);
|
||
|
|
s2 += speed*sin((direction-motorPos[1])*M_PI/180);
|
||
|
|
s3 += speed*sin((direction-motorPos[2])*M_PI/180);
|
||
|
|
|
||
|
|
if(ABS(s1) > 1.0) {
|
||
|
|
s1 /= ABS(s1);
|
||
|
|
s2 /= ABS(s1);
|
||
|
|
s3 /= ABS(s1);
|
||
|
|
}
|
||
|
|
if(ABS(s2) > 1.0) {
|
||
|
|
s1 /= ABS(s2);
|
||
|
|
s2 /= ABS(s2);
|
||
|
|
s3 /= ABS(s2);
|
||
|
|
}
|
||
|
|
if(ABS(s3) > 1.0) {
|
||
|
|
s1 /= ABS(s3);
|
||
|
|
s2 /= ABS(s3);
|
||
|
|
s3 /= ABS(s3);
|
||
|
|
}
|
||
|
|
|
||
|
|
motors[0]->setSpeed(int(s1*255));
|
||
|
|
motors[1]->setSpeed(int(s2*255));
|
||
|
|
motors[2]->setSpeed(int(s3*255));
|
||
|
|
}
|