Diff: main.cpp

Revision:

1:eb812097900f

Parent:

0:f48055d44399

Child:

2:7f48771d8c52

diff -r f48055d44399 -r eb812097900f main.cpp
--- a/main.cpp	Sat Jul 23 03:42:44 2016 +0000
+++ b/main.cpp	Sat Jul 23 10:46:02 2016 +0000
@@ -10,59 +10,62 @@
 VNH5019 motor(p23,p22,p25,p21,p24,p26);
 DigitalIn compass[4] = {p12, p11, p8, p7};
 
+// direction of robot
 int robot_compass()
 {
     int c = 0;
 
+    // convert from base 2 to base 10
     if(compass[0]) c += 1;
     if(compass[1]) c += 2;
     if(compass[2]) c += 4;
     if(compass[3]) c += 8;
 
+    // decide number from data sheet
     switch(c) {
-        case 0:     // N
-            return 8;
-        case 1:     // NNE
-            return 11;
-        case 2:     // NE
-            return 3;
-        case 3:     // ENE
-            return 0;
-        case 4:     // E
-            return 7;
-        case 5:     // ESE
-            return 12;
-        case 6:     // SE
-            return 4;
-        case 7:     // SSE
-            return 15;
-        case 8:     // S
-            return 9;
-        case 9:     // SSW
-            return 10;
-        case 10:    // SW
-            return 2;
-        case 11:    // WSW
-            return 1;
-        case 12:    // W
-            return 6;
-        case 13:    // WNW
-            return 13;
-        case 14:    // NW
-            return 5;
-        case 15:    // N
-            return 14;
+        case 0:
+            return 8;   // N
+        case 1:
+            return 11;  // NNE
+        case 2:
+            return 3;   // NE
+        case 3:
+            return 0;   // ENE
+        case 4:
+            return 7;   // E
+        case 5:
+            return 12;  // ESE
+        case 6:
+            return 4;   // SE
+        case 7:
+            return 15;  // SSE
+        case 8:
+            return 9;   // S
+        case 9:
+            return 10;  // SSW
+        case 10:
+            return 2;   // SW
+        case 11:
+            return 1;   // WSW
+        case 12:
+            return 6;   // W
+        case 13:
+            return 13;  // WNW
+        case 14:
+            return 5;   // NW
+        case 15:
+            return 14;  // N
     }
 }
 
+// direction of target from robot
 int target_compass(double x, double y)
 {
     double angle=0.0;
-    if(x == 0.0){
+    if(x == 0.0) {
         if(y >= 0.0) angle = 0.0;
         else  angle = M_PI;
-    }
-    else if(y == 0.0){
+    } else if(y == 0.0) {
         if(x >= 0.0) angle = M_PI / 2.0;
         else angle = 3.0 * M_PI / 2.0;
     }
@@ -71,27 +74,29 @@
 
     angle = angle / M_PI * 180.0;
 
-    if((348.75 <= angle && angle < 360.0)|| (0.0 <= angle && angle < 11.25)) return 0;
-    else if(11.25 <= angle && angle < 33.75) return 1;
-    else if(33.75 <= angle && angle < 56.25) return 2;
-    else if(56.25 <= angle && angle < 78.75) return 3;
-    else if(78.75 <= angle && angle < 101.25) return 4;
-    else if(101.25 <= angle && angle < 123.75) return 5;
-    else if(123.75 <= angle && angle < 146.25) return 6;
-    else if(146.25 <= angle && angle < 168.75) return 7;
-    else if(168.75 <= angle && angle < 191.25) return 8;
-    else if(191.25 <= angle && angle < 213.75) return 9;
-    else if(213.75 <= angle && angle < 236.25) return 10;
-    else if(236.25 <= angle && angle < 258.75) return 11;
-    else if(258.75 <= angle && angle < 281.25) return 12;
-    else if(281.25 <= angle && angle < 303.75) return 13;
-    else if(303.75 <= angle && angle < 326.25) return 14;
-    else if(326.25 <= angle && angle < 348.75) return 15;
+    // decide number like robot compass
+    if((348.75 <= angle && angle < 360.0)|| (0.0 <= angle && angle < 11.25)) return 0;  // N
+    else if(11.25 <= angle && angle < 33.75) return 1;      // NNE
+    else if(33.75 <= angle && angle < 56.25) return 2;      // NE
+    else if(56.25 <= angle && angle < 78.75) return 3;      // ENE
+    else if(78.75 <= angle && angle < 101.25) return 4;     // E
+    else if(101.25 <= angle && angle < 123.75) return 5;    // ESE
+    else if(123.75 <= angle && angle < 146.25) return 6;    // SE
+    else if(146.25 <= angle && angle < 168.75) return 7;    // SSE
+    else if(168.75 <= angle && angle < 191.25) return 8;    // S
+    else if(191.25 <= angle && angle < 213.75) return 9;    // SSW
+    else if(213.75 <= angle && angle < 236.25) return 10;   // SW
+    else if(236.25 <= angle && angle < 258.75) return 11;   // WSW
+    else if(258.75 <= angle && angle < 281.25) return 12;   // W
+    else if(281.25 <= angle && angle < 303.75) return 13;   // WNW
+    else if(303.75 <= angle && angle < 326.25) return 14;   // NW
+    else if(326.25 <= angle && angle < 348.75) return 15;   // NNW
     else return 100;
 
     return angle;
 }
 
+// f->forward, r->right turn, l->left turn, b->back, s->stop
 char robot_action(int robot, int target)
 {
 
@@ -99,39 +104,67 @@
     if(robot <= target) n = target - robot;
     else n = target - robot + 16;
 
+    // direction of targeet and robot are same
     if(n == 0) {
         return 'f';
+        // direction of target is right from direction of robot
     } else if(0 < n && n <= 8) {
         return 'r';
+        // direction of target is left from direction of robot
     } else if(8 < n && n < 16) {
         return 'l';
     } else return 'b';
 }
 
-// f->front, r->right, l->left, b->back, s->stop
+// f->forward, r->right turn, l->left turn, b->back, s->stop
 void motor_control(char c, int speed)
 {
+    // front
     if(c == 'f') {
         motor.changeSpeed(1, speed, 1, speed);
     }
 
+    // right
     if(c == 'r') {
         motor.changeSpeed(1, speed, 1, speed-32);
     }
 
+    // left
     if(c == 'l') {
         motor.changeSpeed(1, speed-32, 1, speed);
     }
 
+    // back
     if(c == 'b') {
-        motor.changeSpeed(2, speed, 2, speed);
+        motor.changeSpeed(2, speed, 2, speed-32);
     }
 
+    // stop
     if(c == 's') {
         motor.changeSpeed(0, speed, 0, speed);
     }
 }
 
+// back (5 seconds) -> forward (10 seconds)
+void stack_action(int speed)
+{
+
+    Timer back_Timer;
+    const int back_time = 5000;
+    Timer forward_Timer;
+    const int forward_time = 10000;
+
+    back_Timer.start();
+
+    while(back_Timer.read_ms() <= back_time) {
+        motor_control('b', speed);
+    }
+    
+    while(forward_Timer.read_ms() <= forward_time){
+        motor_control('f', speed);
+    }
+}
+
 int main()
 {
     pc.baud(57600);
@@ -141,29 +174,34 @@
     Adafruit_GPS myGPS(gps_Serial);
     myGPS.begin(9600);
 
-    Timer GPS_Timer;
-    const int GPS_Time = 1000;
-
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
 
-    const double target_x = 139.939017, target_y = 37.522390;    // target gps 
-    double robot_x, robot_y;    // robot gps
-    double sub_x =0.0, sub_y=0.0, distance = 500.0;
+    // GPS
+    const double target_x = 139.939017, target_y = 37.522390;
+    double robot_x, robot_y;
+    double sub_x =0.0, sub_y=0.0, distance = 500.0, d = 500.0;
+    int compass = 0;
+    // action of robot
     char action = 'S';
+    // speed of robot
     int speed = 128;
+    int state = 0;
+
+    // Timer
+    Timer GPS_Timer;
+    const int GPS_Time = 1000;
     Timer action_Timer;
     const int action_time = 500;
     Timer stack_Timer;
     const int stack_time = 5000;
-    int a = 0;
-    int state = 0;
 
     xbee.printf("start\n");
 
     GPS_Timer.start();
     action_Timer.start();
+    stack_Timer.start();
 
     while(true) {
 
@@ -182,6 +220,7 @@
         }
 
         motor_control(action, speed);
+
         myGPS.read();
         if ( myGPS.newNMEAreceived() ) {
             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
@@ -189,35 +228,55 @@
             }
         }
 
+        // get GPS
         if (GPS_Timer.read_ms() >= GPS_Time) {
             GPS_Timer.reset();
             if(myGPS.fix) {
                 robot_x = (double)myGPS.longitudeH +(double)(myGPS.longitudeL / 10000.0 / 60.0);
                 robot_y = (double)myGPS.latitudeH + (double)(myGPS.latitudeL / 10000.0 / 60.0);
 
+                // distance of robot and target
                 sub_x = (target_x*10000.0 - robot_x*10000.0) * 11.11350;
                 sub_y = (target_y*10000.0 - robot_y*10000.0) * 9.11910;
+                d = distance;
                 distance = sqrt(sub_x*sub_x + sub_y*sub_y);
             }
         }
 
-        if(distance <= 1.0) {
-            state = 2;
-            xbee.printf("***STOP***\n");
-            //motor_control('s', 128);
-            //break;
-        }
-
+        // action
         if(action_Timer.read_ms() >= action_time) {
             action_Timer.reset();
+
+            // action of robot
             action = robot_action(robot_compass(),target_compass(sub_x, sub_y));
-            if(distance < 10.0) speed = 64;
-            xbee.printf("%d (%f, %f) distance:%f, target:%d, robot:%d, action:%c\n",
-                        state, robot_x, robot_y, distance, target_compass(sub_x, sub_y), robot_compass(), action);
+            // speed of robot
+            if(distance < 5.0) speed = 64;
+            else speed = 128;
+
+            xbee.printf("(%f, %f) distance:%f, target:%d, robot:%d, action:%c\n",
+                        robot_x, robot_y, distance, target_compass(sub_x, sub_y), robot_compass(), action);
             pc.printf("(%f, %f) distance:%f, target:%d, robot:%d, action:%c\n",
-                      robot_x, robot_y, distance, target_compass(sub_x, sub_y), robot_compass(), action);
+                      robot_x, robot_y, distance, d, target_compass(sub_x, sub_y), robot_compass(), action);
         }
 
+        // this positon is a target
+        if(distance <= 0.5) {
+            action = 's';
+            xbee.printf("***STOP***\n");
+        }
+
+        // stack check
+        if(!(d-0.15 <= distance && distance <= d+0.15)) {
+            if(compass != robot_compass()) {
+                stack_Timer.reset();
+            }
+        }
+
+        // stack
+        if(stack_Timer.read_ms() >= stack_time) {
+            if(state == 1)stack_action(speed);
+            stack_Timer.reset();
+        }
     }
     return 0;
 }
\ No newline at end of file