drive: fix to accept tilt values in the [0,360] range

pull/1/head
EmaMaker 2021-06-01 16:17:52 +02:00
parent 25d30047c2
commit 6c81dbf998
1 changed files with 2 additions and 1 deletions

View File

@ -94,8 +94,9 @@ void DriveController::drive(int dir, int speed, int tilt){
// Calculate position error relative to the 0
delta = CURRENT_DATA_READ.IMUAngle;
if(delta > 180) delta = delta - 360;
input = delta;
if(tilt > 180) tilt = tilt - 360;
setpoint = tilt;
if(pid->Compute()){