David Lopes
dfgs
Revision 0:22b8909431a3, committed 2015-07-05
- Comitter:
- scarcyon
- Date:
- Sun Jul 05 09:51:29 2015 +0000
- Commit message:
- ass
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi.lib Sun Jul 05 09:51:29 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/scarcyon/code/m3pii/#3fc886273b7a
--- /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();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Jul 05 09:51:29 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7cff1c4259d7 \ No newline at end of file