README.md

Robot arduino sketch

Some motor-control code for our robot kit.

robot.ino

// edit this function to control your robot.
// this function will get called over and over again.
void loop() {
  // here are some example function calls:
  forward(20);  // move forward for 20 ticks.
  pivot_left(10);  // pivot left for 10 ticks.
  spin_right(10);  // spin right for 8 ticks.
  flash_led();  // blink the LED three times.
}

// if you want to change the speed of your motors, edit this variable.
int motor_speed = 255;  // choose a value from 0 to 255.




// --- robot control functions ---
// you can call these functions to control your robot.
// you don't need to edit any of these functions.

void forward(int ticks) {
  move_ticks(ticks, 1, 1, motor_speed);
}

void backward(int ticks) {
  move_ticks(ticks, -1, -1, motor_speed);
}

void brake() {
  rmotor_brake();
  lmotor_brake();
}

void spin_right(int ticks) {
  move_ticks(ticks, 1, -1, motor_speed);
}

void spin_left(int ticks) {
  move_ticks(ticks, -1, 1, motor_speed);
}

void pivot_right(int ticks) {
  move_ticks(ticks, 1, 0, motor_speed);
}

void pivot_left(int ticks) {
  move_ticks(ticks, 0, 1, motor_speed);
}




// Below this point is a bunch of implementation details which you can ignore.
// You don't need to edit any of this.

// --- pin assignments ---

// H-bridge connections:
#define IN1 11  // the right motor
#define IN2 10  // the right motor
#define IN3 9   // the left motor
#define IN4 6   // the left motor

#define ENCODER_WHEEL_RIGHT 3
#define ENCODER_WHEEL_LEFT 2

// --- motor control functions ---

void rmotor_forward(int pwm) {
  analogWrite(IN1, pwm);
  digitalWrite(IN2, LOW);  
}

void lmotor_forward(int pwm) {
  analogWrite(IN3, pwm);
  digitalWrite(IN4, LOW);  
}

void rmotor_backward(int pwm) {
  digitalWrite(IN1, LOW);
  analogWrite(IN2, pwm);
}

void lmotor_backward(int pwm) {
  digitalWrite(IN3, LOW);
  analogWrite(IN4, pwm);  
}
void rmotor_brake() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
}

void lmotor_brake() {
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// --- tick-based motor control functions ---

int g_target_lticks = 0;
int g_target_rticks = 0;

int g_actual_lticks = 0;
int g_actual_rticks = 0;

void move_ticks(int ticks, int ldir, int rdir, int pwm) {
  bool lflip_flop = false;
  bool rflip_flop = false;
  bool lon = false;
  bool ron = false;
  
  for (int i=0; i < ticks; i++) {
    g_target_lticks += ldir;
    g_target_rticks += rdir;
    
    ldir == 1 ? lmotor_forward(pwm) : lmotor_backward(pwm);
    lon = true;
    rdir == 1 ? rmotor_forward(pwm) : rmotor_backward(pwm);
    ron = true;
    
    while (
      (((g_target_lticks - g_actual_lticks) * ldir) > 0) ||
      (((g_target_rticks - g_actual_rticks) * rdir) > 0)
    ) {
      int lencoder = digitalRead(ENCODER_WHEEL_LEFT);
      if (lencoder && !lflip_flop) {
        lflip_flop = true;
      } else if (!lencoder && lflip_flop) {
        g_actual_lticks += ldir;
        lflip_flop = false;      
      }

      int rencoder = digitalRead(ENCODER_WHEEL_RIGHT);
      if (rencoder && !rflip_flop) {
        rflip_flop = true;
      } else if (!rencoder && rflip_flop) {
        g_actual_rticks += rdir;
        rflip_flop = false;      
      }
    
      if (      
        (((g_target_lticks - g_actual_lticks) * ldir) <= 0) && lon
      ) {
        lmotor_brake();
        lon = false;
      }

      if (      
        (((g_target_rticks - g_actual_rticks) * rdir) <= 0) && ron
      ) {
        rmotor_brake();
        ron = false;
      }
    }
  }
  brake();
}

// --- misc ---

void flash_led() {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(100);
  digitalWrite(LED_BUILTIN, LOW);
  delay(100);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(100);
  digitalWrite(LED_BUILTIN, LOW);
  delay(100);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(100);
  digitalWrite(LED_BUILTIN, LOW);
  delay(100);
}

// --- main arduino funcions ---

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENCODER_WHEEL_LEFT, INPUT);
  pinMode(ENCODER_WHEEL_RIGHT, INPUT);
}