Robot Brain 0.1

gistfile1.ino

// Robot Brain v0.1
//
// kelexel@github
//
// this sketch is meant to control the robotbase dual h-bridge
// http://www.seeedstudio.com/depot/datasheet/L298%20Dual%20H-Bridge%20Motor%20Driver%20datasheet.pdf
// the bot has a seeeduino, an xbee shield and xbee serie2
// right now the remote control is just an xbee plugged to a pc.

// Motors
int motor_left[] = {4, 5}; //I3 I4
int pwm_left = 10; // EB
int motor_right[] = {6, 7}; //I1 I2
int pwm_right = 11; // EA
int highSpeed = 255;
int lowSpeed = 200;

// Command keys
int command;  // command received via serial
int directionKeys[] = {122, 100, 115, 113, 32}; // accepted commands: z, d, s, q, space
int dir_up = 122;

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

  // command received by serial
  int command;  

  // Setup motors
  int i;
  for(i = 0; i < 2; i++){
    pinMode(motor_left[i], OUTPUT);
    pinMode(motor_right[i], OUTPUT);
  }
}


void loop() {
  if (Serial.available() > 0) {  
  // read the incoming byte:  
    command = Serial.read();
    if (command != -1) {
      int i=0;
      for (i=0; i<5; i++) {
        if (command == directionKeys[i]) {
          switch (command) {
            case 122: //up
              motor_stop();
              drive_forward();
            break;
            case 100: //right
              motor_stop();
              turn_right();
            break;
            case 115: //down
              motor_stop();
              drive_backward();
            break;
            case 113: //left
              motor_stop();
              turn_left();
            break;
            case 32: //space
              motor_stop();
            break;
          }
//          Serial.print(command);
        }
      }
    }
  }
  delay(100);
}

// ————————————————————————— Drive

void motor_stop(){
  // left motor
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], LOW);
  analogWrite(pwm_left, 0);

  // right motor  
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], LOW);
  analogWrite(pwm_right, 0);

  delay(25);
}

void drive_forward(){
  // left motor
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], HIGH);
  analogWrite(pwm_left, highSpeed);

  // right motor
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], HIGH);
  analogWrite(pwm_right, highSpeed);
}

void drive_backward(){
  // left motor
  digitalWrite(motor_left[0], HIGH);
  digitalWrite(motor_left[1], LOW);
  analogWrite(pwm_left, highSpeed);

  // right motor
  digitalWrite(motor_right[0], HIGH);
  digitalWrite(motor_right[1], LOW);
  analogWrite(pwm_right, highSpeed);
}

void turn_left(){
  // left motor
  digitalWrite(motor_left[0], HIGH);
  digitalWrite(motor_left[1], LOW);
  analogWrite(pwm_left, highSpeed);

  // right motor
  digitalWrite(motor_right[0], LOW);
  digitalWrite(motor_right[1], HIGH);
  analogWrite(pwm_right, lowSpeed);
}

void turn_right(){
  // left motor
  digitalWrite(motor_left[0], LOW);
  digitalWrite(motor_left[1], HIGH);
  analogWrite(pwm_left, lowSpeed);

  // right motor
  digitalWrite(motor_right[0], HIGH);
  digitalWrite(motor_right[1], LOW);
  analogWrite(pwm_right, highSpeed);
}