MEGACARAZ
PIDs and fun
Dependencies: btbee m3pi_ng mbed FatFileSystemCpp
Diff: USBTest.cpp
- Revision:
- 18:1dc7fdc248aa
- Parent:
- 17:b9608f93ea06
--- a/USBTest.cpp Fri May 29 13:40:56 2015 +0000
+++ b/USBTest.cpp Fri May 29 13:41:58 2015 +0000
@@ -1,289 +1,289 @@
-#include "mbed.h"
-#include "m3pi_ng.h"
-#include "time.h"
-#include "btbee.h"
-#include "MSCFileSystem.h"
-#include <vector>
-
-#define FSNAME "msc"
-MSCFileSystem msc(FSNAME);
-
-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 1.0
-#define MIN 0
-
-// PID terms
-#define P_TERM 1.5
-#define I_TERM 0
-#define D_TERM 25
-
-int main() {
+ #include "mbed.h"
+ #include "m3pi_ng.h"
+ #include "time.h"
+ #include "btbee.h"
+ #include "MSCFileSystem.h"
+ #include <vector>
+
+ #define FSNAME "msc"
+ MSCFileSystem msc(FSNAME);
+
+ 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 1.0
+ #define MIN 0
+
+ // PID terms
+ #define P_TERM 1.5
+ #define I_TERM 0
+ #define D_TERM 25
-
-while(1) {
-
-
- btbee.reset();
- m3pi_pb.mode(PullUp);
+ int main() {
+
+
+ while(1) {
+
+
+ btbee.reset();
+ m3pi_pb.mode(PullUp);
+
+ m3pi.printf("Wait 4");
+ m3pi.locate(0,1);
+ m3pi.printf("PC");
+
+ while(m3pi_pb) {
+ m3pi_led[0]=!m3pi_led[0];
+ wait(3);
+ btbee.printf("\n");
+ btbee.printf("PC connected. Press the button \n");
+ }
+
+ m3pi.cls();
+
+ Timer LapTimer;
+ Timer LoopTime;
+ Timer WriteTime;
+ 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)
+ {
+ 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);
+ }
- m3pi.printf("Wait 4");
- m3pi.locate(0,1);
- m3pi.printf("PC");
+ m3pi.locate(0,1);
+ m3pi.printf("Line PID");
- while(m3pi_pb) {
- m3pi_led[0]=!m3pi_led[0];
- wait(3);
- btbee.printf("\n");
- btbee.printf("PC connected. Press the button \n");
- }
-
- m3pi.cls();
+ wait(2.0);
- Timer LapTimer;
- Timer LoopTime;
- Timer WriteTime;
- 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)
- {
- 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);
- }
-
- m3pi.locate(0,1);
- m3pi.printf("Line PID");
-
- wait(2.0);
-
- btbee.printf("Now calibrating \n");
-
- m3pi.sensor_auto_calibrate();
+ btbee.printf("Now calibrating \n");
+
+ m3pi.sensor_auto_calibrate();
+
+ btbee.printf("Finished calibrating \n");
+
- btbee.printf("Finished calibrating \n");
+ float right;
+ float left;
+ float current_pos_of_line = 0.0;
+ float previous_pos_of_line = 0.0;
+ float derivative,proportional,integral = 0;
+ double power;
+ float speed = MAX;
+ int LapTest[5];
+ int s1, s2, s3, s4, s5;
+ int counter = -1;
+ int checkvar = 0;
+ double Time1, Time2, Time3, Time4, Time5, TimeAve;
+ vector < double > ZeTimes(2);
+ vector < vector < double > > Data;
+ double stuff;
+ double junk;
+ double timer;
+ double TimeToWrite;
+ int loopcounter = 0;
+
+
+ btbee.printf("\n");
+ btbee.printf("Now starting \n");
+
+ while (1) {
+
+ LoopTime.start();
-
- float right;
- float left;
- float current_pos_of_line = 0.0;
- float previous_pos_of_line = 0.0;
- float derivative,proportional,integral = 0;
- double power;
- float speed = MAX;
- int LapTest[5];
- int s1, s2, s3, s4, s5;
- int counter = -1;
- int checkvar = 0;
- double Time1, Time2, Time3, Time4, Time5, TimeAve;
- vector < double > ZeTimes(2);
- vector < vector < double > > Data;
- double stuff;
- double junk;
- double timer;
- double TimeToWrite;
- int loopcounter = 0;
-
-
- btbee.printf("\n");
- btbee.printf("Now starting \n");
-
- while (1) {
-
- LoopTime.start();
-
-
- // Get the position of the line.
- current_pos_of_line = m3pi.line_position();
-
- stuff = current_pos_of_line;
-
- proportional = current_pos_of_line;
- //fprintf(fp, "%f", current_pos_of_line);
- //fprintf(fp, " ");
-
- // 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;
+
+ // Get the position of the line.
+ current_pos_of_line = m3pi.line_position();
+
+ stuff = current_pos_of_line;
+
+ proportional = current_pos_of_line;
+ //fprintf(fp, "%f", current_pos_of_line);
+ //fprintf(fp, " ");
+
+ // 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)) ;
- if (left < MIN)
- left = MIN;
- else if (left > MAX)
- left = MAX;
+ // Compute new speeds
+ right = speed+power;
+ left = speed-power;
- // set speed
- m3pi.left_motor(left);
- m3pi.right_motor(right);
+ // 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);
- m3pi.calibrated_sensor(LapTest);
+ s1 = LapTest[0];
+ s2 = LapTest[1];
+ s3 = LapTest[2];
+ s4 = LapTest[3];
+ s5 = LapTest[4];
+
+ if (s1 > 500 and s5 > 500) {
+ checkvar = 1;
+ }
+
+ if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
+ LapTimer.start();
+ }
+
+ if (s1 < 500 and s5 < 500 and checkvar == 1) {
+ counter = counter + 1;
+ m3pi.cls();
+ m3pi.printf("%d", counter);
+ checkvar = 0;
+ switch (counter) {
+ case 1: {
+ Time1 = LapTimer.read();
+ LapTimer.reset();
+ LapTimer.start();
+ btbee.printf("Lap 1: %f \n", Time1);
+
+ break;
+ }
+ case 2: {
+ Time2 = LapTimer.read();
+ LapTimer.reset();
+ LapTimer.start();
+ btbee.printf("Lap 2: %f \n", Time2);
+
+ break;
+ }
+ case 3: {
+ Time3 = LapTimer.read();
+ LapTimer.reset();
+ LapTimer.start();
+ btbee.printf("Lap 3: %f \n", Time3);
+
+ break;
+ }
+ case 4: {
+ Time4 = LapTimer.read();
+ LapTimer.reset();
+ LapTimer.start();
+ btbee.printf("Lap 4: %f \n", Time4);
+
+ break;
+ }
+ case 5: {
+ Time5 = LapTimer.read();
+ LapTimer.reset();
+ LapTimer.start();
+ btbee.printf("Lap 5: %f \n", Time5);
+
+ break;
+ }
+ }
+ }
+
+ ZeTimes[0]= stuff;
+ ZeTimes[1] = power;
+ Data.push_back(ZeTimes);
+
+ loopcounter++;
+
+
+ timer = LoopTime.read();
+
+ if(timer < 0.005) {
+ wait(0.005 - timer);
+ }
+
+ timer = 0;
+ LoopTime.reset();
+
+ if (loopcounter == 10) {
+ m3pi.stop();
+ m3pi.cls();
+ m3pi.locate(0,0);
+ m3pi.printf("Ave time");
+ TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
+ m3pi.locate(0,1);
+ m3pi.printf("%f", TimeAve);
+ btbee.printf("Average time per lap: %f ", TimeAve);
+ btbee.printf("\n");
+ btbee.printf("Printing to USB");
+
+ WriteTime.start();
+
+ DIR *d;
+ struct dirent *p;
+ d = opendir("/" FSNAME);
+ FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
+ if ( fp == NULL ){
+ m3pi.leds(1);
+ }
+
+ for(int i = 0; i < 10; i++) {
+ for( int j = 0; j < 2; j++) {
+ junk = Data[i][j];
+ fprintf(fp,"%f ", junk);
+ }
+ fprintf(fp,"\r\n");
+ }
+ m3pi.cls();
+ TimeToWrite = WriteTime.read();
+ btbee.printf("%f \n", TimeToWrite);
+ WriteTime.reset();
+
+ btbee.printf("Printed");
+ m3pi.cls();
+ m3pi.printf("Printed");
+
+ fclose(fp);
+
+ exit(1);
+ }
- s1 = LapTest[0];
- s2 = LapTest[1];
- s3 = LapTest[2];
- s4 = LapTest[3];
- s5 = LapTest[4];
-
- if (s1 > 500 and s5 > 500) {
- checkvar = 1;
- }
-
- if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
- LapTimer.start();
- }
-
- if (s1 < 500 and s5 < 500 and checkvar == 1) {
- counter = counter + 1;
- m3pi.cls();
- m3pi.printf("%d", counter);
- checkvar = 0;
- switch (counter) {
- case 1: {
- Time1 = LapTimer.read();
- LapTimer.reset();
- LapTimer.start();
- btbee.printf("Lap 1: %f \n", Time1);
-
- break;
- }
- case 2: {
- Time2 = LapTimer.read();
- LapTimer.reset();
- LapTimer.start();
- btbee.printf("Lap 2: %f \n", Time2);
-
- break;
- }
- case 3: {
- Time3 = LapTimer.read();
- LapTimer.reset();
- LapTimer.start();
- btbee.printf("Lap 3: %f \n", Time3);
-
- break;
- }
- case 4: {
- Time4 = LapTimer.read();
- LapTimer.reset();
- LapTimer.start();
- btbee.printf("Lap 4: %f \n", Time4);
-
- break;
- }
- case 5: {
- Time5 = LapTimer.read();
- LapTimer.reset();
- LapTimer.start();
- btbee.printf("Lap 5: %f \n", Time5);
-
- break;
- }
- }
- }
+ }
- ZeTimes[0]= stuff;
- ZeTimes[1] = power;
- Data.push_back(ZeTimes);
-
- loopcounter++;
-
-
- timer = LoopTime.read();
-
- if(timer < 0.005) {
- wait(0.005 - timer);
- }
+ }
- timer = 0;
- LoopTime.reset();
-
- if (loopcounter == 10) {
- m3pi.stop();
- m3pi.cls();
- m3pi.locate(0,0);
- m3pi.printf("Ave time");
- TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
- m3pi.locate(0,1);
- m3pi.printf("%f", TimeAve);
- btbee.printf("Average time per lap: %f ", TimeAve);
- btbee.printf("\n");
- btbee.printf("Printing to USB");
-
- WriteTime.start();
-
- DIR *d;
- struct dirent *p;
- d = opendir("/" FSNAME);
- FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
- if ( fp == NULL ){
- m3pi.leds(1);
- }
-
- for(int i = 0; i < 10; i++) {
- for( int j = 0; j < 2; j++) {
- junk = Data[i][j];
- fprintf(fp,"%f ", junk);
- }
- fprintf(fp,"\r\n");
- }
- m3pi.cls();
- TimeToWrite = WriteTime.read();
- btbee.printf("%f \n", TimeToWrite);
- WriteTime.reset();
-
- btbee.printf("Printed");
- m3pi.cls();
- m3pi.printf("Printed");
-
- fclose(fp);
-
- exit(1);
}
-
-
-
-}
-
-}
-
-}