diff --git a/kuukar/kuukar_motion.py b/kuukar/kuukar_motion.py index e6cd4da..ba66708 100644 --- a/kuukar/kuukar_motion.py +++ b/kuukar/kuukar_motion.py @@ -100,10 +100,21 @@ class motion: if speed>99: speed=99 elif speed<-99: speed= -99 - self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed)) - self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed)) - self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed) - self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed) + if (rightSpeed - leftSpeed) > 0: + self.motor_write(MOTOR_FL_F, MOTOR_FL_R, -1*abs(leftSpeed)) + self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed)) + self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed) + self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed) + elif (rightSpeed - leftSpeed) < 0: + self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed)) + self.motor_write(MOTOR_FR_F, MOTOR_FR_R, -1*abs(rightSpeed)) + self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed) + self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed) + else: + self.motor_write(MOTOR_FL_F, MOTOR_FL_R, abs(leftSpeed)) + self.motor_write(MOTOR_FR_F, MOTOR_FR_R, abs(rightSpeed)) + self.motor_write(MOTOR_RL_F, MOTOR_RL_R, speed) + self.motor_write(MOTOR_RR_F, MOTOR_RR_R, speed)