Diff: Robot-bae8eb81a9d7/main.cpp

Revision:

0:0f96a93fbf39

Child:

1:2db3ccba18c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot-bae8eb81a9d7/main.cpp	Wed Jun 03 13:32:30 2015 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "btbee.h"
+#include "m3pi_ng.h"
+#include <vector>
+#include <string>
+m3pi robot;
+Timer timer;
+Timer time_wait;
+#define MAX 0.75
+#define MIN 0
+
+#define P_TERM 2.5
+#define I_TERM 2
+#define D_TERM 23
+
+void turnRight();
+void turnLeft();
+void goStraight();
+
+int main()
+{
+
+    robot.sensor_auto_calibrate();
+    wait(2.0);
+
+    float right;
+    float left;
+    //float current_pos[5];
+    float current_pos = 0.0;
+    float previous_pos =0.0;
+    float derivative, proportional, integral = 0;
+    float power;
+    float speed = MAX;
+    string turns;
+    int direction = 0;
+    int x [5];
+    bool passed = false;
+
+
+    
+    int Run = 1;
+    vector< vector<string> > Lookup(6, vector<string>(6));
+    
+    Lookup[1][4] = "LLS";
+    
+
+    
+
+
+    time_wait.start();
+    
+    turns = Lookup[0][1];
+    while(Run) {
+        time_wait.reset();
+        //Get raw sensor values
+        
+        robot.calibrated_sensor(x);
+
+
+
+        //Check to make sure battery isn't low
+        if (robot.battery() < 2.4) {
+            timer.stop();
+            break;
+        }
+
+        
+    
+        if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
+            
+            if(turns.at(direction) == 'L'){
+                robot.stop();
+                wait(1);
+                turnLeft();
+                robot.stop();
+                goStraight();
+                wait(1);
+            }
+            else if(turns.at(direction) == 'R'){
+                robot.stop();
+                wait(1);
+                turnRight();
+                robot.stop();
+                goStraight();
+                wait(1);
+            }
+            else{
+                robot.stop();
+                wait(1);
+                goStraight();
+                wait(1);
+            }
+            robot.printf("Size: %d", direction);
+            ++direction;
+            robot.calibrated_sensor(x);
+        }
+        else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
+            robot.stop();
+            break;
+        }
+        else if (x[0]>300 || x[4]>300 && passed)
+            passed = true;
+        else
+            passed = false;
+
+
+        // Get the position of the line.
+        current_pos = robot.line_position();
+        proportional = current_pos;
+
+        derivative = current_pos - previous_pos;
+
+
+        //compute the integral
+        integral =+ proportional;
+
+        //remember the last position.
+        previous_pos = current_pos;
+
+        // compute the power
+        power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+        //computer 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>MIN)
+            left = MAX;
+
+        //set speed
+
+        robot.left_motor(left);
+        robot.right_motor(right);
+
+        wait((5-time_wait.read_ms())/1000);
+    }
+
+
+
+    robot.stop();
+
+    char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
+                  ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
+                  ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
+                  ,'G','8','E','8','D','8','C','4'
+                 };
+    int numb = 59;
+
+    robot.playtune(hail,numb);
+
+
+}
+
+
+void turnRight(){
+    Timer turner;
+    turner.start();
+    while (turner.read_ms() < 105){
+            robot.right(.5);
+    }
+    turner.stop();
+}
+
+void turnLeft(){
+    Timer turner;
+    turner.start();
+    while (turner.read_ms() < 105){
+            robot.left(.5);
+    }
+    turner.stop();
+}
+
+void goStraight(){
+    Timer turner;
+    turner.start();
+    while (turner.read_ms() < 50){
+            robot.forward(.5);
+    }
+    turner.stop();
+}
\ No newline at end of file