David Lopes / Mbed 2 deprecated v10

dsa

Dependencies:   m3pi mbed

main.cpp@0:c327ac576b6a, 2015-07-05 (annotated)

Committer:
scarcyon
Date:
Sun Jul 05 09:43:32 2015 +0000
Revision:
0:c327ac576b6a
as

Who changed what in which revision?

UserRevisionLine numberNew contents of line
scarcyon 0:c327ac576b6a 1 #include "mbed.h"
scarcyon 0:c327ac576b6a 2 #include "m3pi.h"
scarcyon 0:c327ac576b6a 3
scarcyon 0:c327ac576b6a 4 #define MAX 0.4
scarcyon 0:c327ac576b6a 5 #define MIN 0
scarcyon 0:c327ac576b6a 6 #define P_TERM 1
scarcyon 0:c327ac576b6a 7 #define I_TERM 0
scarcyon 0:c327ac576b6a 8 #define D_TERM 20
scarcyon 0:c327ac576b6a 9
scarcyon 0:c327ac576b6a 10 AnalogIn ain(p20);
scarcyon 0:c327ac576b6a 11 Serial rn42(p28,p27);
scarcyon 0:c327ac576b6a 12 m3pi m3pi;
scarcyon 0:c327ac576b6a 13
scarcyon 0:c327ac576b6a 14 float right;
scarcyon 0:c327ac576b6a 15 float left;
scarcyon 0:c327ac576b6a 16 float current_pos_of_line = 0.0;
scarcyon 0:c327ac576b6a 17 float previous_pos_of_line = 0.0;
scarcyon 0:c327ac576b6a 18 float derivative,proportional,integral = 0;
scarcyon 0:c327ac576b6a 19 float power;
scarcyon 0:c327ac576b6a 20 float speed = MAX;
scarcyon 0:c327ac576b6a 21 float obstaculo;
scarcyon 0:c327ac576b6a 22 char cmd='s';
scarcyon 0:c327ac576b6a 23 int sensors[5];
scarcyon 0:c327ac576b6a 24 int marcas=0;
scarcyon 0:c327ac576b6a 25 int marca_detectada=0;
scarcyon 0:c327ac576b6a 26 int speed_flag=0;
scarcyon 0:c327ac576b6a 27 int display_flag=0;
scarcyon 0:c327ac576b6a 28 int menu_flag=0;
scarcyon 0:c327ac576b6a 29 int num_marcas=0;
scarcyon 0:c327ac576b6a 30 char cmd_ant='s';
scarcyon 0:c327ac576b6a 31
scarcyon 0:c327ac576b6a 32
scarcyon 0:c327ac576b6a 33
scarcyon 0:c327ac576b6a 34
scarcyon 0:c327ac576b6a 35 void flip(void)
scarcyon 0:c327ac576b6a 36 {
scarcyon 0:c327ac576b6a 37 if (rn42.readable()) {
scarcyon 0:c327ac576b6a 38 cmd_ant=cmd;
scarcyon 0:c327ac576b6a 39 cmd=rn42.getc();
scarcyon 0:c327ac576b6a 40 speed_flag=0;
scarcyon 0:c327ac576b6a 41 display_flag=0;
scarcyon 0:c327ac576b6a 42 menu_flag=0;
scarcyon 0:c327ac576b6a 43 marca_detectada=0;
scarcyon 0:c327ac576b6a 44 }
scarcyon 0:c327ac576b6a 45 }
scarcyon 0:c327ac576b6a 46
scarcyon 0:c327ac576b6a 47 int main()
scarcyon 0:c327ac576b6a 48 {
scarcyon 0:c327ac576b6a 49
scarcyon 0:c327ac576b6a 50 m3pi.sensor_auto_calibrate();
scarcyon 0:c327ac576b6a 51 rn42.baud(115200);
scarcyon 0:c327ac576b6a 52 rn42.attach(&flip,Serial::RxIrq);
scarcyon 0:c327ac576b6a 53 m3pi.cls();
scarcyon 0:c327ac576b6a 54
scarcyon 0:c327ac576b6a 55 while (1) {
scarcyon 0:c327ac576b6a 56
scarcyon 0:c327ac576b6a 57 if(display_flag==0) {
scarcyon 0:c327ac576b6a 58 display_flag=1;
scarcyon 0:c327ac576b6a 59 rn42.putc( cmd );
scarcyon 0:c327ac576b6a 60 m3pi.cls();
scarcyon 0:c327ac576b6a 61 m3pi.printf("%c", cmd);
scarcyon 0:c327ac576b6a 62 }
scarcyon 0:c327ac576b6a 63
scarcyon 0:c327ac576b6a 64 m3pi.locate(0,0);
scarcyon 0:c327ac576b6a 65 if(cmd=='w') {
scarcyon 0:c327ac576b6a 66 m3pi.left_motor(speed);
scarcyon 0:c327ac576b6a 67 m3pi.right_motor(speed);
scarcyon 0:c327ac576b6a 68 } else if(cmd=='d') {
scarcyon 0:c327ac576b6a 69 m3pi.left_motor(0.0);
scarcyon 0:c327ac576b6a 70 m3pi.right_motor(speed);
scarcyon 0:c327ac576b6a 71 } else if(cmd=='a') {
scarcyon 0:c327ac576b6a 72 m3pi.left_motor(speed);
scarcyon 0:c327ac576b6a 73 m3pi.right_motor(0.0);
scarcyon 0:c327ac576b6a 74 } else if(cmd=='x') {
scarcyon 0:c327ac576b6a 75 m3pi.backward(speed);
scarcyon 0:c327ac576b6a 76 } else if(cmd=='+') {
scarcyon 0:c327ac576b6a 77 if(speed_flag==0) {
scarcyon 0:c327ac576b6a 78 speed_flag=1;
scarcyon 0:c327ac576b6a 79 speed+=0.1;
scarcyon 0:c327ac576b6a 80 if(speed>1.0)
scarcyon 0:c327ac576b6a 81 speed=1.0;
scarcyon 0:c327ac576b6a 82 }
scarcyon 0:c327ac576b6a 83 } else if(cmd=='-') {
scarcyon 0:c327ac576b6a 84 if(speed_flag==0) {
scarcyon 0:c327ac576b6a 85 speed_flag=1;
scarcyon 0:c327ac576b6a 86 speed-=0.1;
scarcyon 0:c327ac576b6a 87 if(speed<-1.0)
scarcyon 0:c327ac576b6a 88 speed=-1.0;
scarcyon 0:c327ac576b6a 89 }
scarcyon 0:c327ac576b6a 90 } else if(cmd=='m') {
scarcyon 0:c327ac576b6a 91 if(menu_flag==0) {
scarcyon 0:c327ac576b6a 92 menu_flag=1;
scarcyon 0:c327ac576b6a 93 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" );
scarcyon 0:c327ac576b6a 94 }
scarcyon 0:c327ac576b6a 95 } else if(cmd=='z') {
scarcyon 0:c327ac576b6a 96
scarcyon 0:c327ac576b6a 97 do {
scarcyon 0:c327ac576b6a 98 obstaculo=ain.read();
scarcyon 0:c327ac576b6a 99 m3pi.readsensor(sensors);
scarcyon 0:c327ac576b6a 100 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))) {
scarcyon 0:c327ac576b6a 101 if(marca_detectada==0) {
scarcyon 0:c327ac576b6a 102 marcas++;
scarcyon 0:c327ac576b6a 103 marca_detectada=1;
scarcyon 0:c327ac576b6a 104 }
scarcyon 0:c327ac576b6a 105 } else {
scarcyon 0:c327ac576b6a 106 marca_detectada=0;
scarcyon 0:c327ac576b6a 107 }
scarcyon 0:c327ac576b6a 108
scarcyon 0:c327ac576b6a 109 m3pi.locate(0,0);
scarcyon 0:c327ac576b6a 110 m3pi.printf("m: %d", marcas);
scarcyon 0:c327ac576b6a 111
scarcyon 0:c327ac576b6a 112
scarcyon 0:c327ac576b6a 113 if(obstaculo<0.025)
scarcyon 0:c327ac576b6a 114 m3pi.stop();
scarcyon 0:c327ac576b6a 115 else {
scarcyon 0:c327ac576b6a 116 // Get the position of the line.
scarcyon 0:c327ac576b6a 117 current_pos_of_line = m3pi.line_position();
scarcyon 0:c327ac576b6a 118 proportional = current_pos_of_line;
scarcyon 0:c327ac576b6a 119
scarcyon 0:c327ac576b6a 120 // Compute the derivative
scarcyon 0:c327ac576b6a 121 derivative = current_pos_of_line - previous_pos_of_line;
scarcyon 0:c327ac576b6a 122
scarcyon 0:c327ac576b6a 123 // Compute the integral
scarcyon 0:c327ac576b6a 124 integral += proportional;
scarcyon 0:c327ac576b6a 125
scarcyon 0:c327ac576b6a 126 // Remember the last position.aas
scarcyon 0:c327ac576b6a 127 previous_pos_of_line = current_pos_of_line;
scarcyon 0:c327ac576b6a 128
scarcyon 0:c327ac576b6a 129 // Compute the power
scarcyon 0:c327ac576b6a 130 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
scarcyon 0:c327ac576b6a 131
scarcyon 0:c327ac576b6a 132 // Compute new speeds
scarcyon 0:c327ac576b6a 133 right = speed+power;
scarcyon 0:c327ac576b6a 134 left = speed-power;
scarcyon 0:c327ac576b6a 135
scarcyon 0:c327ac576b6a 136 // limit checks
scarcyon 0:c327ac576b6a 137 if (right < MIN)
scarcyon 0:c327ac576b6a 138 right = MIN;
scarcyon 0:c327ac576b6a 139 else if (right > MAX)
scarcyon 0:c327ac576b6a 140 right = MAX;
scarcyon 0:c327ac576b6a 141
scarcyon 0:c327ac576b6a 142 if (left < MIN)
scarcyon 0:c327ac576b6a 143 left = MIN;
scarcyon 0:c327ac576b6a 144 else if (left > MAX)
scarcyon 0:c327ac576b6a 145 left = MAX;
scarcyon 0:c327ac576b6a 146
scarcyon 0:c327ac576b6a 147 // set speed
scarcyon 0:c327ac576b6a 148 m3pi.left_motor(left);
scarcyon 0:c327ac576b6a 149 m3pi.right_motor(right);
scarcyon 0:c327ac576b6a 150 }
scarcyon 0:c327ac576b6a 151 } while(cmd!='s');
scarcyon 0:c327ac576b6a 152 m3pi.left_motor(0.0);
scarcyon 0:c327ac576b6a 153 m3pi.right_motor(0.0);
scarcyon 0:c327ac576b6a 154 m3pi.cls();
scarcyon 0:c327ac576b6a 155 m3pi.printf("saiu");
scarcyon 0:c327ac576b6a 156 m3pi.locate(0,0);
scarcyon 0:c327ac576b6a 157 } else if(cmd=='s') {
scarcyon 0:c327ac576b6a 158 m3pi.left_motor(0.0);
scarcyon 0:c327ac576b6a 159 m3pi.right_motor(0.0);
scarcyon 0:c327ac576b6a 160 }
scarcyon 0:c327ac576b6a 161 }
scarcyon 0:c327ac576b6a 162 }