diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 23991a0..45df50e 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -66,8 +66,12 @@ void DriveController::drive(int dir, int speed, int tilt){ tilt = tilt > 180 ? tilt - 360 : tilt; //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE? - vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; - vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; + // 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 + // 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)) { vxn = 0; @@ -84,7 +88,7 @@ void DriveController::drive(int dir, int speed, int tilt){ speed3 = -(speed1); speed4 = -(speed2); - // calcola l'errore di posizione rispetto allo 0 + // Calculate position error relative to the 0 delta = CURRENT_DATA_READ.IMUAngle; if(delta > 180) delta = delta - 360; diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp index 7fd606b..f6ee95c 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/position/positionsys_camera.cpp @@ -118,13 +118,16 @@ void PositionSysCamera::CameraPID(){ int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350); 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 + // vx = ((speed * cosins[dir])); + // vy = ((-speed * sins[dir])); - vx = ((speed * cosins[dir])); - vy = ((-speed * sins[dir])); - - CURRENT_DATA_WRITE.addvx = vx; - CURRENT_DATA_WRITE.addvy = vy; + // CURRENT_DATA_WRITE.addvx = vx; + // CURRENT_DATA_WRITE.addvy = vy; } }