Diff: main.cpp

Revision:

0:22b8909431a3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 05 09:51:29 2015 +0000
@@ -0,0 +1,201 @@
+#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','s','s','s','s'};
+int cmd_i=1;
+int cmd_i_ant=0;
+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;
+//float speed=0.2;
+
+
+
+void flip(void)
+{
+    if (rn42.readable()) {
+        cmd_i_ant++;
+        cmd_i_ant=cmd_i_ant%5;
+        cmd_i++;
+        cmd_i=cmd_i%5;
+        cmd[cmd_i]=rn42.getc();
+        speed_flag=0;
+        display_flag=0;
+        menu_flag=0;
+        marca_detectada=0;
+    }
+    //return;
+}
+
+int main()
+{
+
+    m3pi.sensor_auto_calibrate();
+    rn42.baud(115200);
+
+    rn42.attach(&flip,Serial::RxIrq);
+
+    m3pi.cls();
+
+    while (1) {
+        //if (rn42.readable()) {
+        //cmd=rn42.getc();
+        //rn42.putc(cmd);
+        // m3pi.printf("v= %c", cmd);
+
+        if(display_flag==0){
+            display_flag=1;
+        rn42.putc( cmd[cmd_i] );
+        m3pi.cls();
+        m3pi.printf("%c", cmd);
+        }
+
+
+        m3pi.locate(0,0);
+        if(cmd[cmd_i]=='w') {
+            m3pi.left_motor(speed);
+            m3pi.right_motor(speed);
+        } else if(cmd[cmd_i]=='d') {
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(speed);
+        } else if(cmd[cmd_i]=='a') {
+            m3pi.left_motor(speed);
+            m3pi.right_motor(0.0);
+        } else if(cmd[cmd_i]=='x') {
+            m3pi.backward(speed);
+        } else if(cmd[cmd_i]=='+') {
+            if(speed_flag==0) {
+                speed_flag=1;
+                speed+=0.1;
+                if(speed>1.0)
+                    speed=1.0;
+            }
+        } else if(cmd[cmd_i]=='-') {
+            if(speed_flag==0) {
+                speed_flag=1;
+                speed-=0.1;
+                if(speed<-1.0)
+                    speed=-1.0;
+            }
+        } 
+        else if(cmd[cmd_i]=='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" );
+            cmd_i--;
+            if(cmd_i<0)
+                cmd_i=4;
+            cmd_i_ant--;
+            if(cmd_i_ant<0)
+                cmd_i_ant=4;
+            }
+        }else if(cmd[cmd_i]=='z') {
+            //wait(2.0);
+
+            do {
+                //if (rn42.readable()) {
+                // cmd=rn42.getc();
+                //rn42.putc(cmd);}
+                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) {
+                        if(cmd[cmd_i_ant]=='q'){
+                            num_marcas--;
+                            if(num_marcas<=0){
+                                cmd_i_ant++;
+                                cmd_i_ant=cmd_i_ant%5;
+                                cmd_i++;
+                                cmd_i=cmd_i%5;
+                                cmd[cmd_i]='s';
+                            }
+                        }
+                        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[cmd_i]!='s');
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(0.0);
+            m3pi.cls();
+            m3pi.printf("saiu");
+            m3pi.locate(0,0);
+        }  else if(cmd[cmd_i_ant]=='q'){
+            num_marcas=atoi(&cmd[cmd_i]);
+            cmd[cmd_i]='z';
+        }
+        else {
+            m3pi.left_motor(0.0);
+            m3pi.right_motor(0.0);
+        }
+        sleep();
+    } 
+}