Diff: main.cpp

Revision:

0:c327ac576b6a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 05 09:43:32 2015 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+#define MAX 0.4
+#define MIN 0
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+AnalogIn   ain(p20);
+Serial rn42(p28,p27);
+m3pi m3pi;
+
+float right;
+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;
+float obstaculo;
+char cmd='s';
+int sensors[5];
+int marcas=0;
+int marca_detectada=0;
+int speed_flag=0;
+int display_flag=0;
+int menu_flag=0;
+int num_marcas=0;
+char cmd_ant='s';
+
+
+
+
+void flip(void)
+{
+    if (rn42.readable()) {
+        cmd_ant=cmd;
+        cmd=rn42.getc();
+        speed_flag=0;
+        display_flag=0;
+        menu_flag=0;
+        marca_detectada=0;
+    }
+}
+
+int main()
+{
+
+    m3pi.sensor_auto_calibrate();
+    rn42.baud(115200);
+    rn42.attach(&flip,Serial::RxIrq);
+    m3pi.cls();
+
+    while (1) {
+
+        if(display_flag==0) {
+            display_flag=1;
+            rn42.putc( cmd );
+            m3pi.cls();
+            m3pi.printf("%c", cmd);
+        }
+
+        m3pi.locate(0,0);
+        if(cmd=='w') {
+            m3pi.left_motor(speed);
+            m3pi.right_motor(speed);
+        } else if(cmd=='d') {
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(speed);
+        } else if(cmd=='a') {
+            m3pi.left_motor(speed);
+            m3pi.right_motor(0.0);
+        } else if(cmd=='x') {
+            m3pi.backward(speed);
+        } else if(cmd=='+') {
+            if(speed_flag==0) {
+                speed_flag=1;
+                speed+=0.1;
+                if(speed>1.0)
+                    speed=1.0;
+            }
+        } else if(cmd=='-') {
+            if(speed_flag==0) {
+                speed_flag=1;
+                speed-=0.1;
+                if(speed<-1.0)
+                    speed=-1.0;
+            }
+        } else if(cmd=='m') {
+            if(menu_flag==0) {
+                menu_flag=1;
+                rn42.printf( "\nControlo Manual:\n\tw: Frente\n\tx:Tras1\n\td:Direita1\n\ta:Esquerda\n\nControlo Automatico:\n\tz: Activar\n\tx: Ir ate marca numero x\n" );
+            }
+        } else if(cmd=='z') {
+
+            do {
+                obstaculo=ain.read();
+                m3pi.readsensor(sensors);
+                if(((sensors[0]>900 && sensors[1]>900 && sensors[2]>900 && sensors[3]>900) || (sensors[1]>900 && sensors[2]>900 && sensors[3]>900 && sensors[4]>900))) {
+                    if(marca_detectada==0) {
+                        marcas++;
+                        marca_detectada=1;
+                    }
+                } else {
+                    marca_detectada=0;
+                }
+
+                m3pi.locate(0,0);
+                m3pi.printf("m: %d", marcas);
+
+
+                if(obstaculo<0.025)
+                    m3pi.stop();
+                else {
+                    // 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.aas
+                    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);
+                }
+            } while(cmd!='s');
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(0.0);
+            m3pi.cls();
+            m3pi.printf("saiu");
+            m3pi.locate(0,0);
+        } else if(cmd=='s') {
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(0.0);
+        }
+    }
+}