David Lopes
/
v10
dsa
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?
User | Revision | Line number | New 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 | } |