SHARE
TWEET

i2c_motor

a guest May 22nd, 2019 87 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Wire.h>
  2. #include <TimedAction.h>  // for test case
  3.  
  4. #define ADDR_SLAVE 0x04
  5. #define ID_MOTOR_LEFT 0
  6. #define ID_MOTOR_RIGHT 1
  7.  
  8. // left
  9. // IN1:+    IN2:LOW -> forward
  10. // IN1:LOW  IN2:+ -> backward
  11. #define IN1 11
  12. #define IN2 12
  13. // right
  14. // IN3:LOW  IN4:+ -> forward
  15. // IN3:+    IN4:LOW -> backward
  16. #define IN3 10
  17. #define IN4 9
  18.  
  19. #define ENC_A_LEFT 2
  20. #define ENC_B_LEFT 4
  21. #define ENC_A_RIGHT 3
  22. #define ENC_B_RIGHT 5
  23.  
  24. // PPR (Pulses Per Revolution)
  25. const float ENCODEROUTPUT = 11.0;
  26. const float diameter = 0.035;
  27. const float CIRCUMFERENCE = M_PI * diameter;
  28.  
  29. volatile long pul_enc_l = 0;
  30. volatile long pul_enc_r = 0;
  31. volatile int ts_prev_enc = 0, ts_now_enc = 0, ts_interval = 1000;
  32.  
  33. #define LEN_DATA_I2C 2
  34. uint8_t data_i2c[LEN_DATA_I2C];
  35. int id = 0;
  36. const int MAX_MOTOR_VAL = 220;
  37. const float SCALE_MOTOR = 2 * (float)MAX_MOTOR_VAL / 255.0;
  38. const float SHIFT_MOTOR = -(float)MAX_MOTOR_VAL;
  39. // left/right motor values
  40. float val_ml = 0;
  41. float val_mr = 0;
  42. const int len_foo = 8;
  43. int foo[len_foo] = {0, 60, 100, 60, 0, -60, -100, -60};
  44. int id_foo = 0;
  45.  
  46. void motorControlTest();
  47. void outputEncoder();
  48.  
  49. TimedAction testThread = TimedAction(3000, motorControlTest);
  50. TimedAction testOutputThread = TimedAction(500, outputEncoder);
  51.  
  52. void setup() {
  53.   Serial.begin(115200);
  54.  
  55.   // Init I2C as slave
  56.   Wire.begin(ADDR_SLAVE);
  57.  
  58.   // define callbacks for i2c communication
  59.   Wire.onReceive(receiveData);
  60.   Wire.onRequest(sendData);
  61.  
  62.   pinMode(IN1, OUTPUT);
  63.   pinMode(IN2, OUTPUT);
  64.   pinMode(IN3, OUTPUT);
  65.   pinMode(IN4, OUTPUT);
  66.  
  67.   pinMode(ENC_A_LEFT, INPUT);
  68.   pinMode(ENC_B_LEFT, INPUT);
  69.   pinMode(ENC_A_RIGHT, INPUT);
  70.   pinMode(ENC_B_RIGHT, INPUT);
  71.  
  72.   // initialize hardware interrupts
  73.   attachInterrupt(digitalPinToInterrupt(ENC_A_LEFT), leftEncoderEvent, CHANGE);
  74.   attachInterrupt(digitalPinToInterrupt(ENC_A_RIGHT), rightEncoderEvent, CHANGE);
  75.  
  76.   testThread.check();
  77. }
  78. // encoder event for the interrupt call
  79. void leftEncoderEvent() {
  80.   if (digitalRead(ENC_A_LEFT) == HIGH) {
  81.     if (digitalRead(ENC_B_LEFT) == LOW) {
  82.       pul_enc_l++;
  83.     } else {
  84.       pul_enc_l--;
  85.     }
  86.   } else {
  87.     if (digitalRead(ENC_B_LEFT) == LOW) {
  88.       pul_enc_l--;
  89.     } else {
  90.       pul_enc_l++;
  91.     }
  92.   }
  93. }
  94.  
  95. // encoder event for the interrupt call
  96. void rightEncoderEvent() {
  97.   if (digitalRead(ENC_A_RIGHT) == HIGH) {
  98.     if (digitalRead(ENC_B_RIGHT) == LOW) {
  99.       pul_enc_r++;
  100.     } else {
  101.       pul_enc_r--;
  102.     }
  103.   } else {
  104.     if (digitalRead(ENC_B_RIGHT) == LOW) {
  105.       pul_enc_r--;
  106.     } else {
  107.       pul_enc_r++;
  108.     }
  109.   }
  110. }
  111.  
  112. void loop() {
  113.   // put your main code here, to run repeatedly:
  114.   testThread.check();
  115.   testOutputThread.check();
  116. }
  117.  
  118. // callback for received data
  119. void receiveData(int byteCount) {
  120.   id = 0;
  121.   while (Wire.available()) {
  122.     if (id >= LEN_DATA_I2C)
  123.       Serial.println("[ERR] data out of buffer.");
  124.  
  125.     data_i2c[id++] = Wire.read();
  126.   }
  127.   for (int i = 0; i < LEN_DATA_I2C; ++i) {
  128.     Serial.println(data_i2c[i]);
  129.   }
  130.   //  motorControl();
  131. }
  132.  
  133. // callback for sending data
  134. void sendData() {
  135.   Wire.write(data_i2c, LEN_DATA_I2C);
  136.   Serial.print("sendData from Arduino: ");
  137.   for (int i = 0; i < LEN_DATA_I2C; ++i) {
  138.     Serial.print(data_i2c[i]);
  139.     Serial.print(" ");
  140.   }
  141.   Serial.println();
  142. }
  143.  
  144. void outputEncoder() {
  145.   ts_now_enc = millis();
  146.   if (ts_now_enc - ts_prev_enc > ts_interval) {
  147.     ts_prev_enc = ts_now_enc;
  148.  
  149.     float val = 60.0 / (2.0 * ENCODEROUTPUT * (float)ts_now_enc);
  150.     float rpm_l = (float)pul_enc_l * val;
  151.     float rpm_r = (float)pul_enc_r * val;
  152.     pul_enc_l = pul_enc_r = 0;
  153.  
  154.     Serial.print("[encoder] l: ");
  155.     Serial.print(pul_enc_l);
  156.     Serial.print(" r: ");
  157.     Serial.println(pul_enc_r);
  158.     Serial.print("est. rpm_l: ");
  159.     Serial.print(rpm_l);
  160.     Serial.print(" rpm_r: ");
  161.     Serial.println(rpm_r);
  162.   }
  163. }
  164.  
  165. void motorControlTest() {
  166.   Serial.println("M<ooootor> PWM set to: ");
  167.   Serial.println(foo[id_foo]);
  168.   if (foo[id_foo] >= 0) {
  169.     analogWrite(IN1, LOW);
  170.     analogWrite(IN2, foo[id_foo]);
  171.     analogWrite(IN3, foo[id_foo]);
  172.     analogWrite(IN4, LOW);
  173.   } else {
  174.     analogWrite(IN1, -foo[id_foo]);
  175.     analogWrite(IN2, LOW);
  176.     analogWrite(IN3, LOW);
  177.     analogWrite(IN4, -foo[id_foo]);
  178.   }
  179.   id_foo = id_foo + 1 == len_foo ? 0 : id_foo + 1;
  180.   Serial.println("M<ooootor> - END");
  181.   return;
  182.  
  183.   for (int i = 0; i < 5; ++i) {
  184.     analogWrite(IN1, 100);
  185.     analogWrite(IN2, LOW);
  186.     analogWrite(IN3, LOW);
  187.     analogWrite(IN4, 100);
  188.     delay(3000);
  189.  
  190.     analogWrite(IN1, 200);
  191.     analogWrite(IN2, LOW);
  192.     analogWrite(IN3, LOW);
  193.     analogWrite(IN4, 200);
  194.     delay(3000);
  195.  
  196.     analogWrite(IN1, 100);
  197.     analogWrite(IN2, LOW);
  198.     analogWrite(IN3, LOW);
  199.     analogWrite(IN4, 100);
  200.     delay(3000);
  201.  
  202.     analogWrite(IN1, LOW);
  203.     analogWrite(IN2, LOW);
  204.     analogWrite(IN3, LOW);
  205.     analogWrite(IN4, LOW);
  206.     delay(5000);
  207.  
  208.     analogWrite(IN1, LOW);
  209.     analogWrite(IN2, 100);
  210.     analogWrite(IN3, 100);
  211.     analogWrite(IN4, LOW);
  212.     delay(3000);
  213.  
  214.     analogWrite(IN1, LOW);
  215.     analogWrite(IN2, 200);
  216.     analogWrite(IN3, 200);
  217.     analogWrite(IN4, LOW);
  218.     delay(3000);
  219.  
  220.     analogWrite(IN1, LOW);
  221.     analogWrite(IN2, 100);
  222.     analogWrite(IN3, 100);
  223.     analogWrite(IN4, LOW);
  224.     delay(3000);
  225.  
  226.     analogWrite(IN1, LOW);
  227.     analogWrite(IN2, LOW);
  228.     analogWrite(IN3, LOW);
  229.     analogWrite(IN4, LOW);
  230.     delay(3000);
  231.   }
  232. }
  233.  
  234. void motorControl() {
  235.   val_ml = (float)data_i2c[ID_MOTOR_LEFT] * SCALE_MOTOR + SHIFT_MOTOR;
  236.   val_mr = (float)data_i2c[ID_MOTOR_RIGHT] * SCALE_MOTOR + SHIFT_MOTOR;
  237.   val_ml = (val_ml < MAX_MOTOR_VAL ? val_ml : MAX_MOTOR_VAL) > -MAX_MOTOR_VAL ? val_ml : -MAX_MOTOR_VAL;
  238.   val_mr = (val_mr < MAX_MOTOR_VAL ? val_mr : MAX_MOTOR_VAL) > -MAX_MOTOR_VAL ? val_mr : -MAX_MOTOR_VAL;
  239.  
  240.   //  Serial.print("motor val: ");
  241.   //  Serial.print(val_ml);
  242.   //  Serial.print(" ");
  243.   //  Serial.println(val_mr);
  244.  
  245.   // output motor
  246.   if (val_ml >= 0.0) {
  247.     analogWrite(IN1, (uint8_t)val_ml);
  248.     digitalWrite(IN2, LOW);
  249.   } else {
  250.     analogWrite(IN1, LOW);
  251.     digitalWrite(IN2, (uint8_t) - val_ml);
  252.   }
  253.  
  254.   if (val_mr >= 0.0) {
  255.     analogWrite(IN3, LOW);
  256.     digitalWrite(IN4, (uint8_t)val_mr);
  257.   } else {
  258.     analogWrite(IN3, (uint8_t) - val_mr);
  259.     digitalWrite(IN4, LOW);
  260.   }
  261. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top