David Lopes / Mbed 2 deprecated v30

dfgs

Dependencies:   m3pii mbed

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

Committer:
scarcyon
Date:
Sun Jul 05 09:51:29 2015 +0000
Revision:
0:22b8909431a3
ass

Who changed what in which revision?

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