lline/drive.c

   1 #include "motor.h"
   2
   3 void drive_setLR(drive_t *drive, int16_t l, int16_t r) {
   4   motor_set(drive->left,l);
   5   motor_set(drive->right,r);
   6 }
   7
   8 void drive_set(drive_t *drive, int16_t velocity, int16_t turn) {
   9   int16_t l,r;
  10
  11   l = velocity + turn;
  12   r = velocity - turn;
  13
  14   int16_t largest = max(abs(l),abs(r));
  15   if (largest > MOTOR_SPEED_MAX) {
  16     // a*(c/d) = (a*c)/d
  17     l = ((int32_t)l * MOTOR_SPEED_MAX) / largest;
  18     r = ((int32_t)r * MOTOR_SPEED_MAX) / largest;
  19   }
  20
  21   setLR(l,r);
  22 }