MEGACARAZ
A code with functions for turning right and left.
Dependencies: btbee m3pi_ng mbed
main.cpp
- Committer:
- morganrose402
- Date:
- 2015-06-08
- Revision:
- 18:acc404d8488c
- Parent:
- 17:4e63e8eacad7
File content as of revision 18:acc404d8488c:
#include "mbed.h"
#include "m3pi_ng.h"
#include "time.h"
#include "btbee.h"
#include <vector>
#include <string>
m3pi m3pi;
btbee btbee;
DigitalIn m3pi_pb(p21);
DigitalIn m3pi_IN[] = {(p12)};
DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
// Minimum and maximum motor speeds
#define MAX 0.5
#define MIN 0
// PID terms
#define P_TERM 1.5
#define I_TERM 0
#define D_TERM 25
float right; // initializes variables
float left;
float current_pos_of_line = 0.0;
float previous_pos_of_line = 0.0;
float derivative,proportional,integral = 0;
float power;
float speed = MAX;
int LapTest[5];
float s1, s2, s3, s4, s5;
Timer TurnTime;
Timer LapTime;
Timer TotalTime;
int counter = 0;
string path;
char dir;
int location;
int goal = 0;
vector < int > command;
int i = 0;
int orders;
vector < string > place(6, "");
vector < vector < string > > rga(6,place);
void RightTurn() {
m3pi.stop();
TurnTime.reset();
TurnTime.start();
while (1) {
m3pi.right_motor(0.45);
m3pi.left_motor(-0.1);
if( TurnTime.read() > 0.2375) {
LapTime.reset();
LapTime.start();
return;
}
}
}
void LeftTurn() {
m3pi.stop();
TurnTime.reset();
TurnTime.start();
while (1) {
m3pi.left_motor(0.45);
m3pi.right_motor(-0.1);
if( TurnTime.read() > 0.2375) {
LapTime.reset();
LapTime.start();
return;
}
}
}
void TurnAround() {
TurnTime.reset();
TurnTime.start();
m3pi.stop();
while (1) {
m3pi.left_motor(0.25);
m3pi.right_motor(-0.25);
if( TurnTime.read() > 0.50) {
m3pi.stop();
return;
}
}
}
void GoStraight() {
TurnTime.reset();
TurnTime.start();
while(1) {
m3pi.right_motor(MAX);
m3pi.left_motor(MAX);
if(TurnTime.read() > 0.02) {
LapTime.reset();
LapTime.start();
return;
}
}
}
bool CrossDetect() {
if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500) {
return(1);
}
else if(s1 > 700 and s2 > 700) {
return(1);
}
else if(s5 > 700 and s4 > 700) {
return(1);
}
else {
return(0);
}
}
vector < int > GetLocation () {
char arr_read[30];
int chars_read1;
int stuff;
char things[1];
int k = 1;
btbee.printf("Enter a command string: \n");
while(1) {
if(btbee.readable()) {
btbee.read_line(arr_read, 30, &chars_read1);
for(int j = 0; j < chars_read1-1; j++) {
things[0] = arr_read[j];
stuff = atoi(things);
btbee.printf("%d",stuff);
if( j != 0 ) {
if( stuff == command.at(j-k) ) {
k++;
}
else {
command.push_back(stuff);
}
}
else if (j == 0) {
command.push_back(stuff);
}
}
command.push_back(7);
return(command);
}
}
}
bool LocTest() {
if( goal == 7 )
return 0;
if( path.at(0) == 'P' ) {
return 1;
}
else {
return 0;
}
}
int main() {
rga.at(0).at(0) = "P";
rga.at(0).at(1) = "LRLLRX";
rga.at(0).at(2) = "LRLSSX";
rga.at(0).at(3) = "LRLSRLX";
rga.at(0).at(4) = "LRRLSLRRRLX";
rga.at(0).at(5) = "LRRLRX";
rga.at(1).at(0) = "LRRLRX";
rga.at(1).at(1) = "P";
rga.at(1).at(2) = "LLSX";
rga.at(1).at(3) = "LLRLX";
rga.at(1).at(4) = "LSRLRRLX";
rga.at(1).at(5) = "LRSLRX";
rga.at(2).at(0) = "SSRLRX";
rga.at(2).at(1) = "SRRX";
rga.at(2).at(2) = "P";
rga.at(2).at(3) = "LLX";
rga.at(2).at(4) = "LRSRLX";
rga.at(2).at(5) = "SSSLRX";
rga.at(3).at(0) = "SRRLLRLRX";
rga.at(3).at(1) = "RLRRX";
rga.at(3).at(2) = "RRX";
rga.at(3).at(3) = "P";
rga.at(3).at(4) = "SSRLX";
rga.at(3).at(5) = "SRLRLX";
rga.at(4).at(0) = "RLLLRSRLLRX";
rga.at(4).at(1) = "RLLRLSRX";
rga.at(4).at(2) = "RLLRLRSX";
rga.at(4).at(3) = "RLSSX";
rga.at(4).at(4) = "P";
rga.at(4).at(5) = "RLLLRLX";
rga.at(5).at(0) = "LRLLRX";
rga.at(5).at(1) = "LRSLRX";
rga.at(5).at(2) = "LRSSSX";
rga.at(5).at(3) = "RLRLSX";
rga.at(5).at(4) = "RLRRRLX";
rga.at(5).at(5) = "P";
btbee.reset();
m3pi_pb.mode(PullUp);
m3pi.printf("Wait 4");
m3pi.locate(0,1);
m3pi.printf("PC");
while(m3pi_pb) { // Loop that runs until user presses button after bluetooth is connected.
m3pi_led[0]=!m3pi_led[0];
wait(3);
btbee.printf("\n");
btbee.printf("PC connected. Press the button \n");
}
m3pi.cls();
// reads battery voltage to screen on start up
char Bat[] = {'V','o','l','t',' ','i','s'};
m3pi.print(Bat,7);
wait(0.75);
m3pi.cls();
float batteryvoltage = m3pi.battery();
char* str = new char[30];
sprintf(str, "%.4g", batteryvoltage);
m3pi.print(str,6);
btbee.printf("Battery voltage is %f \n", batteryvoltage);
wait(0.75);
m3pi.cls();
if (batteryvoltage < 4.2) // exits program if voltage is less than 4.2 V
{
char low[] = {'L','o','w',' ','b','a','t'};
m3pi.print(low,7);
char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
m3pi.playtune(ExitSound,11);
btbee.printf("Battery voltage is too low. Stopping program");
exit(1);
}
command = GetLocation();
location = command.at(i);
i++;
goal = command.at(i);
i++;
btbee.printf("\n");
btbee.printf("Now calibrating \n"); // calibrating robot on the line.
m3pi.sensor_auto_calibrate();
btbee.printf("Finished calibrating \n");
btbee.printf("Robot is at location %d \n", location);
btbee.printf("Robot is going to %d \n", goal);
btbee.printf("\n");
btbee.printf("Now starting \n");
//Countdown
char StartTune[]={'V','1','5','O','5','E','4','R','4','E','4','R','4','E','4','R','4','O','6','E','4','R','4'};
m3pi.playtune(StartTune,23);
m3pi.printf("3");
wait(1);
m3pi.cls();
m3pi.printf("2");
wait(1);
m3pi.cls();
m3pi.printf("1");
wait(1);
m3pi.cls();
m3pi.printf("GO!");
wait(.5);
m3pi.cls();
LapTime.start();
TotalTime.start();
path = rga.at(location-1).at(goal-1);
while (1) {
LapTime.start();
// Get the position of the line.
current_pos_of_line = m3pi.line_position();
proportional = current_pos_of_line;
// Compute the derivative
derivative = current_pos_of_line - previous_pos_of_line;
// Compute the integral
integral += proportional;
// Remember the last position.
previous_pos_of_line = current_pos_of_line;
// Compute the power
power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
// Compute new speeds
right = speed+power;
left = speed-power;
// limit checks
if (right < MIN)
right = MIN;
else if (right > MAX)
right = MAX;
if (left < MIN)
left = MIN;
else if (left > MAX)
left = MAX;
// set speed
m3pi.left_motor(left);
m3pi.right_motor(right);
m3pi.calibrated_sensor(LapTest);
s1 = LapTest[0];
s2 = LapTest[1];
s3 = LapTest[2];
s4 = LapTest[3];
s5 = LapTest[4];
if(CrossDetect() and LapTime > 0.1) {
btbee.printf("\n");
dir = path.at(counter);
btbee.printf("%c\n", dir);
switch(dir) {
case 'R': {
RightTurn();
btbee.printf("Turning right\n");
counter++;
break;
}
case 'L': {
LeftTurn();
btbee.printf("Turning left\n");
counter++;
break;
}
case 'S': {
GoStraight();
btbee.printf("Going straight\n");
counter++;
break;
}
case 'X': {
m3pi.left_motor(0.5);
m3pi.right_motor(0.5);
//wait(0.05);
btbee.printf("Reached goal. Turning around\n");
TurnAround();
location = goal;
goal = command.at(i);
if(goal == 7) {
TotalTime.stop();
m3pi.printf("%f",TotalTime.read());
command.clear();
i = 0;
command = GetLocation();
m3pi.cls();
location = command.at(i);
i++;
goal = command.at(i);
TotalTime.reset();
}
path = rga.at(location-1).at(goal-1);
while(LocTest()) {
i++;
goal = command.at(i);
path = rga.at(location-1).at(goal-1);
}
counter = 0;
i++;
break;
}
}
}
}
}