Guest User

Untitled

a guest
May 21st, 2021
43
5 days
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define DEADZONE 10
  2.  
  3. void loop{
  4.     //int is signed 16 bit
  5.     int x = (analogRead(A0) >> 1) - 256; //map 1024 to [-256 +255]
  6.     int y = (analogRead(A1) >> 1) - 256; //map 1024 to [-256 +255]
  7.  
  8.  
  9.     //implement deadzones
  10.     if ((x < DEADZONE) && (x > -DEADZONE)) {
  11.         x = 0;
  12.     }
  13.  
  14.     if ((y < DEADZONE) && (y > -DEADZONE)) {
  15.         y = 0;
  16.     }
  17.  
  18.     //map to wheels with saturation 255
  19.     int left = (x + y);
  20.     left  = (left > 255) ? 255 : (left < -255) ? -255 : left;
  21.     int right = (y - x);
  22.     right = (right > 255) ? 255 : (right < -255) ? -255 : right;
  23.  
  24.  
  25.     //split into sign and abs value
  26.     uint8_t invert_L, invert_R;
  27.     if (left < 0) {
  28.         left = -left;
  29.         invert_L = 1;
  30.     }
  31.  
  32.     if (right < 0) {
  33.         right = -right;
  34.         invert_R = 1;
  35.     }
  36.  
  37.  
  38.     digitalWrite(motorL_dir, (1 - invert_L));
  39.     analogWrite(motorL_pwm, (uint8_t) left);
  40.     digitalWrite(motorR_dir, (1 - invert_R));
  41.     analogWrite(motorR_pwm, (uint8_t) right);
  42. }
  43.  
RAW Paste Data