/* WHEGS Robot Project
Jérôme Antoons
Loïc Blanc
Tom Christiaens
Eline Maeckelberghe
MA1 Electromechanical Engineering
Mechatronics & Constructions
VUB - ULB
This program should be able to control the entire robot, using servomotors to actuate the robot
and using a distance sensor to detect obstacles if they are present
To do: - Check and exectue motor control
- Determine signals from remote control
*/
#include
Servo LeftMotor;
Servo RigthMotor;
// functions
void controlLoop(int signal);
void wait();
int ObstDetection();
void setVcontrol(int signal);
void stopMotor(int motor);
void RunBackward(int motor);
void RunForward(int motor);
void Reinitialize();
int getIRKey();
// Constant declarations
// Pins used
const int Lmotor = 3; // Left motor pin number, PWM output
const int Rmotor = 5; // Right motor pin number, PWM output
const int encoA1 = 6; // Left motor encoder pin number, digital input
const int encoB1 = 9; // Right motor encoder pin number, digital input
const int encoA2 = 10; // Left motor encoder pin number, digital input
const int encoB2 = 11; // Right motor encoder pin number, digital input
const int RCsensor = 12; // Pin IR receiver for remote control, input, but connected to digital pin!!
const int Distsens = 1; // Obstacle detection sensor pin, analog input
const int FLled = 2; // Pin of led to show that left engine is running forward
const int FRled = 4; // Pin of led to show that right left engine is running forward
const int BLled = 7; // Pin of led to show that left engine is running backward
const int BRled = 8; // Pin of led to show that right engine is running backward
const int led_pin = 13; // "Ready to Recieve" flag, not needed but nice
// Constant for RC
const int StopSignal = 132;
const int start_bit = 2000; // Start bit threshold (Microseconds)
const int bin_1 = 1000; // Binary 1 threshold (Microseconds)
const int bin_0 = 400; // Binary 0 threshold (Microseconds)
const int debug = 0; // Serial connection must be started to debug
const int Tini=500;
// Geometric constants
const double pi=3.1416;
const double Rwhegs = 0.07; // [m]
const int deltaV = 5; // for the increase of Vcontrol
double Vcontrol = 3.0; // km/h
// Obstacle variables
int obstacle=0;
const int limitObst = 200; // value of the Sharp Sensor saying that we are close to an obstacle
// Variables decleration
int RCsignal; // IR signal received by sensor
// value of this signal is coupled to the button that is pressed on the RC.
// this value has to be determined using the program found on internet (Pmalmsten)
// 5 buttons are necessary: stop - left turn - right turn - forward - backward
// Variable for the control of the motors
const int timeLoop = 50; // ms
double previousMillis = 0;
double currentMillis = millis(); // counter on the time
double distRef=0; // angle that the wheels should execute in "timeLoop" duration
const int offset1 = 1500; // corresponds to a null speed => motor are blocked
const int offset2 = 1500;
const int gain1 = 50; // gain of the controller
const int gain2 = 50;
int V=0; // speed that we want to put on the motor
double lastEncoder1=0; // last value given by the encoder
double lastEncoder2=0;
const int numberPulses = 16; // number of pulses for one rotation
// for 1 pulse in A, we can see 4 different steps cfr: http://www.bourns.com/pdfs/3315.pdf
const double angle = 2*pi/numberPulses; // value of the angle for each change in encoder
// Encoder variable
int encoder_A = 0; // variable for the encoder value
int encoder_B = 0;
int encoder_A_prev = 0;
double position_left = 0; // variable for the encoder position value
double position_right = 0;
double deltaPos = angle*Rwhegs; // angle should not change in function of rotation
/////////
// Setup Function
/////////
void setup(){
// set (input and) output pins
// Analog in pins are by default set as input
LeftMotor.attach(Lmotor);
LeftMotor.writeMicroseconds(1500);
RigthMotor.attach(Rmotor);
RigthMotor.writeMicroseconds(1500);
pinMode(FLled, OUTPUT); // LEDs, output
pinMode(BLled, OUTPUT);
pinMode(BRled, OUTPUT);
pinMode(FRled, OUTPUT);
pinMode(RCsensor, INPUT); // IR receiver for the remote control, input
pinMode(encoA1, INPUT); // Encoders, input
pinMode(encoA2, INPUT);
pinMode(encoB1, INPUT);
pinMode(encoB2, INPUT);
pinMode(A1,INPUT); // Analog input proximity sensor
pinMode(led_pin, OUTPUT); // This shows when we are ready to recieve
digitalWrite(led_pin, LOW); // Not ready yet
Vcontrol = Vcontrol/(3.6*Rwhegs);// To go from km/h to rad/s
Serial.begin(9600); // if we want to use the serial communication on the computer, to debug, etc.
}
/////////
// No End Loop
/////////
void loop()
{
currentMillis = (millis()/1000.0);
RCsignal = getIRKey();
setVcontrol(RCsignal);
controlLoop(RCsignal);
}
/////////
// Global Control of Motor Loop
/////////
///////// NEED: signal given by the user via the remote control
void controlLoop(int signal){
switch (RCsignal)
{
case 129:
//start moving forward untill stop is pressed OR sensor detects object it cannot overcome
// run left engine forward and right one backwar (due to disposition of engines)
obstacle = ObstDetection(); // check if obstacle is present or not
while (RCsignal != StopSignal && obstacle != 1) // if one of the two condition is met: stop this loop
{
RunForward(Lmotor);
RunBackward(Rmotor);
RCsignal = getIRKey(); // communication from user (stop, speed change)
obstacle = ObstDetection();
setVcontrol(RCsignal);
wait(); // to have constant duration loops
}
if (RCsignal == StopSignal) // put the speed of the motors at 0 to stop the engines
{
stopMotor(Lmotor);
stopMotor(Rmotor);
}
if (obstacle == 1)
{
stopMotor(Lmotor);
stopMotor(Rmotor);
}
break;
case 135:
//start moving backward untill stop is pressed, no need to detect obstacle (sensor only on the front of the robot)
// run left engine backward and right one forward (due to disposition of engines)
while (RCsignal != StopSignal)
{
RunBackward(Lmotor);
RunForward(Rmotor);
wait();
RCsignal = getIRKey();
setVcontrol(RCsignal);
}
if (RCsignal == StopSignal) // put the speed of the motors at 0 to stop the engines
{
stopMotor(Lmotor);
stopMotor(Rmotor);
}
break;
case 131:
//start turning leftward untill stop is pressed
// run both engines forward (due to disposition of engines)
while (RCsignal != StopSignal)
{
RunForward(Lmotor);
RunForward(Rmotor);
wait();
RCsignal = getIRKey();
setVcontrol(RCsignal);
}
if (RCsignal == StopSignal) // put the speed of the motors at 0 to stop the engines
{
stopMotor(Lmotor);
stopMotor(Rmotor);
}
break;
case 133:
//start turning rightward untill stop is pressed
// run both engines backward (due to disposition of engines)
while (RCsignal != StopSignal)
{
RunBackward(Lmotor);
RunBackward(Rmotor);
wait();
RCsignal = getIRKey();
setVcontrol(RCsignal);
}
if (RCsignal == StopSignal) // put the speed of the motors at 0 to stop the engines
{
stopMotor(Lmotor);
stopMotor(Rmotor);
}
break;
case 244: // set the speed of the robot
if (Vcontrol <= (255-deltaV))
Vcontrol += deltaV;
break;
case 245:
if(Vcontrol >= deltaV)
Vcontrol -= deltaV;
break;
}
}
/////////
// Waiting Function
/////////
//////// REMARK: will wait until 50 ms passes since last use of this function
//////////////// will allow a time control on the duration of the program flow
void wait()
{
while ((currentMillis - previousMillis) < (timeLoop/1000.0))
{ // double values and transformation in seconds are used to be able to stock longer duration
currentMillis = (millis()/1000.0); // update the current time cursor
} // wait before going out of the running motor loop
previousMillis = currentMillis;
currentMillis = (millis()/1000.0);
}
/////////
// Speed Setting Function
/////////
///////// NEED: value corresponding to the signal given by the user via the remote control
void setVcontrol(int signal)
{
// Maximal value for Vcontrol is 255, lowest is deltaV corresponding to the incremental value
if (signal == 244 && Vcontrol <= (255-deltaV)) // Signal of 244 corresponds to volume up arrow
Vcontrol += deltaV;
else if (signal == 245 && Vcontrol >= deltaV) // Signal of 245 corresponds to volume down arrow
Vcontrol -= deltaV;
}
/////////
// Stopping Motor Function
/////////
///////// NEED: value corresponding to the motor which should stop (Lmotor for left, Rmotor for right)
void stopMotor(int motor)
{
if (motor == Lmotor)
{
LeftMotor.writeMicroseconds(offset1); // give only the offset to stop the motor => speed corresponding is null
digitalWrite(FLled, LOW); // switch off the LEDs corresponding to this action
digitalWrite(BLled, LOW);
}
else
{
RigthMotor.writeMicroseconds(offset2); // give only the offset to stop the motor => speed corresponding is null
digitalWrite(FRled, LOW); // switch off the LEDs corresponding to this action
digitalWrite(BRled, LOW);
}
}
/////////
// Running Forward Function
/////////
///////// NEED: value corresponding to the motor which should go forward (Lmotor for left, Rmotor for right)
void RunForward(int motor){
distRef=Rwhegs*Vcontrol*timeLoop/255.0*3; // corresponds to distance crossed in timeLoop seconds (50ms)
// is maximal speed is 1 rad/sec (for Vcontrol = 255)
if (motor == Lmotor){
digitalWrite(FLled, HIGH); // shows the running of this function by ligthing the LEDs
digitalWrite(BLled, LOW);
int dist=(encoderRead(1) - lastEncoder1);
lastEncoder1=encoderRead(1);
V=offset1+gain1*(distRef-dist); // V= V0 + K*e : proportional controller
LeftMotor.writeMicroseconds(V);
}
else {
digitalWrite(FRled, HIGH);
digitalWrite(BRled, LOW);
int dist=(encoderRead(2) - lastEncoder2);
lastEncoder2=encoderRead(2);
V=offset2+gain2*(distRef-dist); // V= V0 + K*e : proportional controller
RigthMotor.writeMicroseconds(V);
}
}
/////////
// Running Backward Function
/////////
///////// NEED: value corresponding to the motor which should go backward (Lmotor for left, Rmotor for right)
void RunBackward(int motor)
{
distRef=Rwhegs*Vcontrol*timeLoop/255*3; // corresponds to distance crossed in timeLoop seconds (50ms)
// is maximal speed is 1 rad/sec (for Vcontrol = 255)
if (motor == Lmotor){
digitalWrite(FLled, LOW); // shows the running of this function by ligthing the LEDs
digitalWrite(BLled, HIGH);
int dist=(encoderRead(1) - lastEncoder1);
if (dist<0)
dist = 0-dist; // as going backward: distance between two measurements can be negative
lastEncoder1=encoderRead(1);
V=offset1-gain1*(distRef-dist); // V= V0 + K*e : proportional controller
LeftMotor.writeMicroseconds(V);
}
else {
digitalWrite(FRled, LOW);
digitalWrite(BRled, HIGH);
int dist=(encoderRead(2) - lastEncoder2);
if (dist<0)
dist = 0-dist;
lastEncoder2=encoderRead(2);
V=offset2-gain2*(distRef-dist); // V= V0 + K*e : proportional controller
RigthMotor.writeMicroseconds(V);
}
}
/////////
// Encoder Reading Function
/////////
///////// NEED: Value corresponding to encoder (1 for left motor and 2 for right motor
///////// RETURN: Total distance crossed by this side of robot
double encoderRead(int enco)
{
double output=0;
if (enco==1)
{
encoder_A = digitalRead(encoA1); // Read encoder pins
encoder_B = digitalRead(encoB1);
if((!encoder_A) && (encoder_A_prev)){
// A has gone from high to low : change is perceived
if(encoder_B) { // B is high so clockwise
position_left = position_left + deltaPos;
}
else { // B is low so counter-clockwise
position_left = position_left - deltaPos;
}
}
encoder_A_prev = encoder_A;
output = position_left;
}
else if (enco==2)
{
encoder_A = digitalRead(encoA2); // Read encoder pins
encoder_B = digitalRead(encoB2);
if((!encoder_A) && (encoder_A_prev)){
// A has gone from high to low : change is perceived
if(encoder_B) { // B is high so clockwise
position_right = position_right + deltaPos;
}
else { // B is low so counter-clockwise
position_right = position_right - deltaPos;
}
}
encoder_A_prev = encoder_A;
output = position_right;
}
return output;
}
/////////
// Obstacle Detection Function
/////////
///////// RETURN: Presence of obstacle: 0(no obstacle) or 1(obstacle)
int ObstDetection()
{
int presence=0;
int Obst = analogRead(A1); // Sharp Sensor on analog input (A1 pin)
if (Obst > limitObst) // Statement to detect object, depends on sensor reading
presence=1;
return presence;
}
/////////
// Blinking LED Function
/////////
void Reinitialize(){
// blink with leds to show reinitialization
digitalWrite(FRled, LOW);
digitalWrite(BRled, LOW);
digitalWrite(FLled, LOW);
digitalWrite(BLled, LOW);
delay(Tini);
digitalWrite(FRled, HIGH);
digitalWrite(BRled, HIGH);
digitalWrite(FLled, HIGH);
digitalWrite(BLled, HIGH);
delay(Tini);
digitalWrite(FRled, LOW);
digitalWrite(BRled, LOW);
digitalWrite(FLled, LOW);
digitalWrite(BLled, LOW);
}
/////////
// Remote Control Signal Interpreter Function
/////////
///////// RETURN: Value corresponding to the button pressed on the remote control
int getIRKey() {
int result;
int data[12];
digitalWrite(led_pin, HIGH); //Ok, it is ready to recieve
int val = pulseIn(RCsensor, LOW);
if (val > 2200)
{
data[0] = pulseIn(RCsensor, LOW); // Start measuring bits, I only want low pulses
data[1] = pulseIn(RCsensor, LOW); // only duration of low pulses are needed to
data[2] = pulseIn(RCsensor, LOW); // interpret the corresponding value (0 or 1)
data[3] = pulseIn(RCsensor, LOW);
data[4] = pulseIn(RCsensor, LOW);
data[5] = pulseIn(RCsensor, LOW);
data[6] = pulseIn(RCsensor, LOW);
data[7] = pulseIn(RCsensor, LOW);
data[8] = pulseIn(RCsensor, LOW);
data[9] = pulseIn(RCsensor, LOW);
data[10] = pulseIn(RCsensor, LOW);
data[11] = pulseIn(RCsensor, LOW);
digitalWrite(led_pin, LOW);
for(int i=0;i<11;i++)
{
if(data[i] > bin_1) { //is it a 1?
data[i] = 1;
} else {
if(data[i] > bin_0) { //is it a 0?
data[i] = 0;
} else {
data[i] = 2; //Flag the data as invalid; I don't know what it is!
}
}
}
for(int i=0;i<11;i++) { //Pre-check data for errors
if(data[i] > 1) {
return -1; //Return -1 on invalid data
}
}
result = 0;
int seed = 1;
for(int i=0;i<11;i++)
{ //Convert bits to integer
if(data[i] == 1) {
result += seed;
}
seed = seed * 2;
}
}
return result; //Return key number
}