Skip to content

Instantly share code, notes, and snippets.

@bebbi
Created July 21, 2015 15:03
Show Gist options
  • Save bebbi/54ee0cca7eb4bf9f742c to your computer and use it in GitHub Desktop.
Save bebbi/54ee0cca7eb4bf9f742c to your computer and use it in GitHub Desktop.
// 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));
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment