EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

disable vector sum code for now 

calculations are a bitty crappy, think I found the bug for now (check added comments)
code_midgen
ema.coletta@gmail.com 2020-10-31 15:37:05 +01:00
parent bc040bd084
commit f0a8ad6808
2 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/motors_movement/drivecontroller.cpp
View File

@ -66,8 +66,12 @@ void DriveController::drive(int dir, int speed, int tilt){
tilt = tilt > 180 ? tilt - 360 : tilt; tilt = tilt > 180 ? tilt - 360 : tilt;
//TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE? //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
// vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
// vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
vx = ((speed * cosins[dir]));
vy = ((-speed * sins[dir]));
if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
vxn = 0; vxn = 0;
@ -84,7 +88,7 @@ void DriveController::drive(int dir, int speed, int tilt){
speed3 = -(speed1); speed3 = -(speed1);
speed4 = -(speed2); speed4 = -(speed2);
// calcola l'errore di posizione rispetto allo 0 // Calculate position error relative to the 0
delta = CURRENT_DATA_READ.IMUAngle; delta = CURRENT_DATA_READ.IMUAngle;
if(delta > 180) delta = delta - 360; if(delta > 180) delta = delta - 360;

13
src/position/positionsys_camera.cpp
View File

@ -118,13 +118,16 @@ void PositionSysCamera::CameraPID(){
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350); int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
drive->prepareDrive(dir, speed, 0); drive->prepareDrive(dir, speed, 0);
//Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
//and check the notes in drivecontroller for the other stuff to comment and uncomment
//TODO: add complementary filter on this speed if we keep using it //TODO: add complementary filter on this speed if we keep using it
// vx = ((speed * cosins[dir]));
// vy = ((-speed * sins[dir]));
vx = ((speed * cosins[dir])); // CURRENT_DATA_WRITE.addvx = vx;
vy = ((-speed * sins[dir])); // CURRENT_DATA_WRITE.addvy = vy;
CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy;
} }
} }