![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
dfgs
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(); + } +}