i2c_motor

#include <Wire.h>
#include <TimedAction.h>  // for test case

#define ADDR_SLAVE 0x04
#define ID_MOTOR_LEFT 0
#define ID_MOTOR_RIGHT 1

// left
// IN1:+    IN2:LOW -> forward
// IN1:LOW  IN2:+ -> backward
#define IN1 11
#define IN2 12
// right
// IN3:LOW  IN4:+ -> forward
// IN3:+    IN4:LOW -> backward
#define IN3 10
#define IN4 9

#define ENC_A_LEFT 2
#define ENC_B_LEFT 4
#define ENC_A_RIGHT 3
#define ENC_B_RIGHT 5

// PPR (Pulses Per Revolution)
const float ENCODEROUTPUT = 11.0;
const float diameter = 0.035;
const float CIRCUMFERENCE = M_PI * diameter;

volatile long pul_enc_l = 0;
volatile long pul_enc_r = 0;
volatile int ts_prev_enc = 0, ts_now_enc = 0, ts_interval = 1000;

#define LEN_DATA_I2C 2
uint8_t data_i2c[LEN_DATA_I2C];
int id = 0;
const int MAX_MOTOR_VAL = 220;
const float SCALE_MOTOR = 2 * (float)MAX_MOTOR_VAL / 255.0;
const float SHIFT_MOTOR = -(float)MAX_MOTOR_VAL;
// left/right motor values
float val_ml = 0;
float val_mr = 0;
const int len_foo = 8;
int foo[len_foo] = {0, 60, 100, 60, 0, -60, -100, -60};
int id_foo = 0;

void motorControlTest();
void outputEncoder();

TimedAction testThread = TimedAction(3000, motorControlTest);
TimedAction testOutputThread = TimedAction(500, outputEncoder);

void setup() {
  Serial.begin(115200);

  // Init I2C as slave
  Wire.begin(ADDR_SLAVE);

  // define callbacks for i2c communication
  Wire.onReceive(receiveData);
  Wire.onRequest(sendData);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(ENC_A_LEFT, INPUT);
  pinMode(ENC_B_LEFT, INPUT);
  pinMode(ENC_A_RIGHT, INPUT);
  pinMode(ENC_B_RIGHT, INPUT);

  // initialize hardware interrupts
  attachInterrupt(digitalPinToInterrupt(ENC_A_LEFT), leftEncoderEvent, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_A_RIGHT), rightEncoderEvent, CHANGE);

  testThread.check();
}
// encoder event for the interrupt call
void leftEncoderEvent() {
  if (digitalRead(ENC_A_LEFT) == HIGH) {
    if (digitalRead(ENC_B_LEFT) == LOW) {
      pul_enc_l++;
    } else {
      pul_enc_l--;
    }
  } else {
    if (digitalRead(ENC_B_LEFT) == LOW) {
      pul_enc_l--;
    } else {
      pul_enc_l++;
    }
  }
}

// encoder event for the interrupt call
void rightEncoderEvent() {
  if (digitalRead(ENC_A_RIGHT) == HIGH) {
    if (digitalRead(ENC_B_RIGHT) == LOW) {
      pul_enc_r++;
    } else {
      pul_enc_r--;
    }
  } else {
    if (digitalRead(ENC_B_RIGHT) == LOW) {
      pul_enc_r--;
    } else {
      pul_enc_r++;
    }
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  testThread.check();
  testOutputThread.check();
}

// callback for received data
void receiveData(int byteCount) {
  id = 0;
  while (Wire.available()) {
    if (id >= LEN_DATA_I2C)
      Serial.println("[ERR] data out of buffer.");

    data_i2c[id++] = Wire.read();
  }
  for (int i = 0; i < LEN_DATA_I2C; ++i) {
    Serial.println(data_i2c[i]);
  }
  //  motorControl();
}

// callback for sending data
void sendData() {
  Wire.write(data_i2c, LEN_DATA_I2C);
  Serial.print("sendData from Arduino: ");
  for (int i = 0; i < LEN_DATA_I2C; ++i) {
    Serial.print(data_i2c[i]);
    Serial.print(" ");
  }
  Serial.println();
}

void outputEncoder() {
  ts_now_enc = millis();
  if (ts_now_enc - ts_prev_enc > ts_interval) {
    ts_prev_enc = ts_now_enc;

    float val = 60.0 / (2.0 * ENCODEROUTPUT * (float)ts_now_enc);
    float rpm_l = (float)pul_enc_l * val;
    float rpm_r = (float)pul_enc_r * val;
    pul_enc_l = pul_enc_r = 0;

    Serial.print("[encoder] l: ");
    Serial.print(pul_enc_l);
    Serial.print(" r: ");
    Serial.println(pul_enc_r);
    Serial.print("est. rpm_l: ");
    Serial.print(rpm_l);
    Serial.print(" rpm_r: ");
    Serial.println(rpm_r);
  }
}

void motorControlTest() {
  Serial.println("M<ooootor> PWM set to: ");
  Serial.println(foo[id_foo]);
  if (foo[id_foo] >= 0) {
    analogWrite(IN1, LOW);
    analogWrite(IN2, foo[id_foo]);
    analogWrite(IN3, foo[id_foo]);
    analogWrite(IN4, LOW);
  } else {
    analogWrite(IN1, -foo[id_foo]);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, -foo[id_foo]);
  }
  id_foo = id_foo + 1 == len_foo ? 0 : id_foo + 1;
  Serial.println("M<ooootor> - END");
  return;

  for (int i = 0; i < 5; ++i) {
    analogWrite(IN1, 100);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, 100);
    delay(3000);

    analogWrite(IN1, 200);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, 200);
    delay(3000);

    analogWrite(IN1, 100);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, 100);
    delay(3000);

    analogWrite(IN1, LOW);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, LOW);
    delay(5000);

    analogWrite(IN1, LOW);
    analogWrite(IN2, 100);
    analogWrite(IN3, 100);
    analogWrite(IN4, LOW);
    delay(3000);

    analogWrite(IN1, LOW);
    analogWrite(IN2, 200);
    analogWrite(IN3, 200);
    analogWrite(IN4, LOW);
    delay(3000);

    analogWrite(IN1, LOW);
    analogWrite(IN2, 100);
    analogWrite(IN3, 100);
    analogWrite(IN4, LOW);
    delay(3000);

    analogWrite(IN1, LOW);
    analogWrite(IN2, LOW);
    analogWrite(IN3, LOW);
    analogWrite(IN4, LOW);
    delay(3000);
  }
}

void motorControl() {
  val_ml = (float)data_i2c[ID_MOTOR_LEFT] * SCALE_MOTOR + SHIFT_MOTOR;
  val_mr = (float)data_i2c[ID_MOTOR_RIGHT] * SCALE_MOTOR + SHIFT_MOTOR;
  val_ml = (val_ml < MAX_MOTOR_VAL ? val_ml : MAX_MOTOR_VAL) > -MAX_MOTOR_VAL ? val_ml : -MAX_MOTOR_VAL;
  val_mr = (val_mr < MAX_MOTOR_VAL ? val_mr : MAX_MOTOR_VAL) > -MAX_MOTOR_VAL ? val_mr : -MAX_MOTOR_VAL;

  //  Serial.print("motor val: ");
  //  Serial.print(val_ml);
  //  Serial.print(" ");
  //  Serial.println(val_mr);

  // output motor
  if (val_ml >= 0.0) {
    analogWrite(IN1, (uint8_t)val_ml);
    digitalWrite(IN2, LOW);
  } else {
    analogWrite(IN1, LOW);
    digitalWrite(IN2, (uint8_t) - val_ml);
  }

  if (val_mr >= 0.0) {
    analogWrite(IN3, LOW);
    digitalWrite(IN4, (uint8_t)val_mr);
  } else {
    analogWrite(IN3, (uint8_t) - val_mr);
    digitalWrite(IN4, LOW);
  }
}