Diff: main3.cpp

Revision:

4:cdd8e61cd8c7

Parent:

0:398afdd73d9e

Child:

5:bc86a4fb4784

--- a/main3.cpp	Mon Nov 23 23:24:35 2015 +0000
+++ b/main3.cpp	Thu Feb 22 08:21:12 2018 +0000
@@ -1,4 +1,3 @@
-
 #include "mbed.h"
 #include "m3pi.h"
 #include "MSCFileSystem.h"
@@ -8,24 +7,245 @@
 Serial xbee(p28,p27);
 DigitalOut resetxbee(p26);
 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
-MSCFileSystem fs("fs"); 
+//MSCFileSystem fs("fs"); 
+Timer t;
+Ticker tick1;
 
 BusOut myleds(LED1, LED2, LED3, LED4);
 
+#define D_TERM  0.0
+#define I_TERM  0.1
+#define I_TERMO 0.1
+#define P_TERM  0.9
+#define MAX     0.3
+#define MIN    -0.2
+
+#define seuil(x)  (x>400 ? 1 : 0)
+
+float current_pos_of_line,
+      previous_pos_of_line,
+      derivate,
+      proportional,
+      power,
+      integral,
+      left,
+      right,
+      speed=0.3;
+
+char chain[10];
+int v;
+      
+unsigned short tabsensor[5];      
+volatile unsigned char sensors;
+
+volatile char flag10ms;
+
+char command=1;
+
+void inter1() {
+     flag10ms=1;
+}
+
+void current_state() {
+    unsigned char i;
+    sensors=0;
+    for(i=0; i<5; i++) {
+        sensors = (sensors << 1) | seuil(tabsensor[i]);
+    }
+}
+
+void step() {
+    m3pi.forward(0.4*speed);
+    wait(0.2);
+    // m3pi.stop();    
+}
+
+/*
+ * Results
+ * 1 -> PID
+ * 2 -> Turn back
+ * 3 -> Turn left
+ * 4 -> Turn right
+ */
+char PIDf(char commande) {
+    if(commande==1) {
+        char result;
+        m3pi.calibrated_sensors(tabsensor);
+        current_state();
+        switch(sensors) {
+            case 0x00:
+                // Deadend
+                m3pi.stop();
+                // Back
+                result = 2;
+                break;
+            case 0x1C: case 0x18: case 0x1E:
+                // Forward/Left or Left Only
+                // m3pi.stop();
+                step();                
+                m3pi.calibrated_sensors(tabsensor);
+                current_state();
+                if ((sensors == 0x00) || (sensors == 0x10)) {
+                    // Turn Left
+                    result = 3;
+                } else {
+                    // Forward
+                    result = 1;
+                }
+                break;
+            case 0x07: case 0x03: case 0x0F:
+                // Forward/Right or Right Only -> RF
+                // m3pi.stop();
+                step();
+                result = 4;
+                break;
+            case 0x1F:
+                // 'T' or Intersection or End -> LR or RFL
+                // m3pi.stop();
+                step();
+                m3pi.calibrated_sensors(tabsensor);
+                current_state();
+                if (sensors == 0x1F) {
+                    // End
+                    result = 0;
+                } else {
+                    // Turn Right
+                    result = 4;
+                }
+                /*
+                if ((sensors == 0x00) || (sensors == 0x01) || (sensors == 0x10) || (sensors == 0x11)) {
+                    // Turn Right
+                    result = 4;
+                } else if (sensors == 0x1F) {
+                    // End
+                    result = 0;
+                } else {
+                    // Turn Right
+                    result = 4;
+                }
+                */
+                break;
+            case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
+                //PID
+                // Get the position of the line
+                current_pos_of_line = m3pi.line_position();
+                proportional = current_pos_of_line;
+                // Compute the derivate
+                derivate = current_pos_of_line - previous_pos_of_line;
+                // Compute the integral
+                integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
+                // Remember the last postion
+                previous_pos_of_line = current_pos_of_line;
+                // Compute the power
+                power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
+                // Compute new speeds
+                right = speed-(power*MAX);
+                left = speed+(power*MAX);
+                // Limits checks on motor control
+                right = (right>MAX ? MAX :(right<MIN ? MIN : right));
+                left = (left>MAX ? MAX :(left<MIN ? MIN : left));
+                // Send command to motors
+                m3pi.left_motor(left);
+                m3pi.right_motor(right);
+                result = 1;
+                break;
+            default:
+                // Faire rien
+                m3pi.stop();
+                result = 0;
+                break;
+        }
+        return result; 
+    }
+}
+
+
+/*
+ * Results
+ * 1 -> PID
+ * 2 -> Turn back
+ * 3 -> Turn left
+ * 4 -> Turn right
+ */
+char turn(char command) {
+    if(command > 1 && command < 5) {
+        char result;
+        m3pi.calibrated_sensors(tabsensor);
+        current_state();
+        switch(command) {
+            case 2:
+                // Turn Back
+                if(sensors != 0x01) {
+                    m3pi.right(speed);
+                    result = 2;
+                } else {
+                    m3pi.right(0.4*speed);
+                    wait(0.12);
+                    m3pi.stop();
+                    result = 1;
+                }
+                break;
+            case 3:
+                // Turn Left
+                if(sensors != 0x10) {
+                    m3pi.left(speed);
+                    result = 3;
+                } else {
+                    m3pi.left(0.4*speed);
+                    wait(0.1);
+                    m3pi.stop();
+                    result = 1;
+                }
+                break;
+            case 4:
+                // Turn Right
+                if(sensors != 0x01) {
+                    m3pi.right(speed);
+                    result = 4;
+                } else {
+                    m3pi.right(0.4*speed);
+                    wait(0.1);
+                    m3pi.stop();
+                    result = 1;
+                }
+                break;  
+        }
+        return result;
+    }
+}
+
+    
 int main() {
-    resetxbee =0;
+    resetxbee=0;
     wait(0.01);
     resetxbee =1;
     
-    FILE *p= fopen("/fs/tt.txt","a+");
+    // FILE *p= fopen("/fs/tt.txt","a+");
     m3pi.sensor_auto_calibrate();
     wait(1.);
+    tick1.attach(&inter1,0.01);
     
-    fprintf(p,"ecrire dans la cle USB\r\n");
-    fclose(p);
-    
+    // fprintf(p,"ecrire dans la cle USB\r\n");
+    // fclose(p);
+
     while(1) {
-        
-        
+        if(flag10ms==1) {
+            switch(command) {
+                case 0:
+                    // Faire Rien
+                    m3pi.stop();
+                    break;
+                case 1:
+                    // PID
+                    command = PIDf(command);
+                    break;
+                case 2: case 3: case 4:
+                    // 2 -> Back
+                    // 3 -> Left
+                    // 4 -> Right
+                    command = turn(command);
+                    break;
+            } 
+        }
     }
-}
+}
\ No newline at end of file