robo_v1b.ino

// a first attempt at arduino and C..
// an abstraction for the robodiy robot - only sets speed for now!

#include <Servo.h>
#include <EEPROM.h>


// (only PWM (~) can be written analogous)
// attention: int calcs -> include .0 if float calc needed, or append (float)

// pin definitions
// only pins 2 and 3 have interrupt possibility on arduino
const int button_left = 2;
const int button_right = 3;
// the interrupt number is not same as the pin number
const int ib_left = 0;
const int ib_right = 1;


const int pin_servo_L = 4;  // Servo 1
const int pin_servo_R = 5;  // Servo 1
const int servo1 = 6;  // Servo 1
const int servo2 = 7;  // Servo 1

// leds  - analogWrite(pin, 0-255) SHOULD allow fading
// 13 would be on-board led
const int led_pin[] = { 8, 9, 10 };
const int led_count = 3;

// analogRead(a2) -> 0-1023
const int poti_l = A1;
const int poti_r = A2;
// line finder
const int linef = A3;

// millisecs from stop to detach servo
const int STOPDETACH = 300;

Servo servo_L;
Servo servo_R;

// calibration values: these will be read from EEPROM.
int servo_l_calib;
int servo_r_calib;

class Robot {
  private:
    // starts by not driving
    // speed goes from -85 to 85 (because of calibration)
    // we don't need a state for speed because we ask it directly
    // from servo port (even in detached state)
    int transition_time = 0;
    // time when state was triggered (for detaching servos)
    long set_speed_time_L = 0;
    long set_speed_time_R = 0;

  public:
    /*
     * set speed, incl detaching
     */
    void set_speed(int l_speed, int r_speed) {
      // normalize speed (85 because of calibration)
      if (l_speed < -85) {
        l_speed = -85;
      } else if (l_speed > 85) {
        l_speed = 85;
      }
      if (r_speed < -85) {
        r_speed = -85;
      } else if (r_speed > 85) {
        r_speed = 85;
      }

      // if speed is 0 and previous speed is 0, nothing to do: return
      if (l_speed == 0 && r_speed == 0 &&
          // works if detached too
          servo_l_calib == servo_L.read() &&
          servo_r_calib == servo_R.read()) {
        return;
      }

      int sL = servo_l_calib + l_speed;
      int sR = servo_r_calib - r_speed;

      // set a new timeout for detach IF servo is not moving.
      if (l_speed) {
        set_speed_time_L = millis();
        // only needed if detached before
        servo_L.attach(pin_servo_L);
      }
      if (r_speed) {
        set_speed_time_R = millis();
        servo_R.attach(pin_servo_R);
      }

      servo_L.write(sL);
      servo_R.write(sR);

      // detach servos if needed
      this->_check_detach();
    }
    /*
     * WIP, just started
     */
    void follow_line() {
      int s_value = analogRead(linef);
      int c_value = analogRead(poti_l);
      int bsl_speed = float(analogRead(poti_r) / 50);

      int s1_value, s2_value;
      if (s_value > c_value)) {
        digitalWrite(led_pin[0], false);
      }
      // ok this is where I stopped
    }
    void _check_detach() {
      long deltatime_L = millis() - set_speed_time_L;
      long deltatime_R = millis() - set_speed_time_R;
      // for each servo, if value == calib_value and >1 sec passed, detach
      if (servo_L.read() == servo_l_calib && deltatime_L > STOPDETACH) {
        servo_L.detach();
      }
      if (servo_R.read() == servo_r_calib && deltatime_R > STOPDETACH) {
        servo_R.detach();
      }
    }
};

Robot robot;

void calibrate() {
  servo_l_calib = (float) 75 + 30 * analogRead(poti_l) / 1023;
  servo_r_calib = (float) 75 + 30 * analogRead(poti_r) / 1023;
  servo_L.write(servo_l_calib);
  servo_R.write(servo_r_calib);
  Serial.print("calib l: "); Serial.print(servo_l_calib);
  Serial.print("calib r: "); Serial.println(servo_r_calib);
}


void setup() {
  Serial.begin(9600);
  Serial.println("hello!");

  servo_l_calib = EEPROM.read(47);
  servo_r_calib = EEPROM.read(48);
  Serial.print("ROM calib l: "); Serial.print(servo_l_calib);
  Serial.print("ROM calib r: "); Serial.println(servo_r_calib);
  delay(1000);


  pinMode(button_left, INPUT);
  pinMode(button_right, INPUT);
  pinMode(button_left, INPUT);
  pinMode(button_right, INPUT);
  pinMode(servo1, OUTPUT);
  pinMode(pin_servo_L, OUTPUT);
  pinMode(servo2, OUTPUT);
  pinMode(pin_servo_R, OUTPUT);

  for(int i = 0; i <= led_count; i++) {
    pinMode(led_pin[i], OUTPUT);
    // digitalWrite(led_pin[i], HIGH);
  }

  attachInterrupt(ib_left, on_button_left, RISING);
  attachInterrupt(ib_right, on_button_right, RISING);

  bool has_calibrated = false;
  servo_L.attach(pin_servo_L);
  servo_R.attach(pin_servo_R);    

  while (digitalRead(button_right)) {
    digitalWrite(led_pin[1], !digitalRead(led_pin[1]));
    has_calibrated = true;
    calibrate();
    delay(100);
  }

  servo_L.detach();
  servo_R.detach();

  if (has_calibrated) {
    EEPROM.write(47, servo_l_calib);
    EEPROM.write(48, servo_r_calib);    
  }
  digitalWrite(led_pin[1], 0);
}


int l_speed = 0;
int r_speed = 0;
int inc = 1;

void loop() {
  delay(10);
  l_speed++;
  r_speed++;

  // accelerate
  if (millis() > 5000) {
    // decelerate
    l_speed = 0;
    r_speed = 0;
  }
  // robot.updateState(state);
  robot.set_speed(l_speed, r_speed);
}

// interrupts for buttons
void on_button_left() {
  delay(50);
  Serial.println("interrupt left");
  digitalWrite(led_pin[0], digitalRead(button_left));
};
void on_button_right() {
  delay(50);
  Serial.println("interrupt right");
  digitalWrite(led_pin[1], digitalRead(button_right));
};