Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@1:fb4277ce4d93, 2014-02-28 (annotated)

Committer:
com3
Date:
Fri Feb 28 10:50:11 2014 +0000
Revision:
1:fb4277ce4d93
Parent:
0:e6d14fec4954
Child:
2:59edff92b599
main;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
com3 0:e6d14fec4954 1 #include "mbed.h"
com3 0:e6d14fec4954 2 #include "TextLCD.h"
com3 0:e6d14fec4954 3 #include "common.h"
com3 0:e6d14fec4954 4 #include <math.h>
com3 0:e6d14fec4954 5 #include <sstream>
com3 0:e6d14fec4954 6
com3 1:fb4277ce4d93 7 #define MOTOR_P 30
com3 1:fb4277ce4d93 8 #define LINE_LP 30
com3 1:fb4277ce4d93 9 #define LINE_FP 40
com3 1:fb4277ce4d93 10 #define LINE_ON 0xFFF0
com3 1:fb4277ce4d93 11 #define LINE_TIME 0.5
com3 0:e6d14fec4954 12 #define R 1.0
com3 1:fb4277ce4d93 13 #define S_MAX 5
com3 1:fb4277ce4d93 14 #define S1 15
com3 1:fb4277ce4d93 15 #define S2 10
com3 1:fb4277ce4d93 16 #define S3 5
com3 1:fb4277ce4d93 17 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
com3 1:fb4277ce4d93 18 #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
com3 0:e6d14fec4954 19
com3 1:fb4277ce4d93 20 DigitalIn sw(p5);
com3 1:fb4277ce4d93 21 DigitalIn start(p29);
com3 0:e6d14fec4954 22 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
com3 0:e6d14fec4954 23 Serial motor(p9,p10);
com3 0:e6d14fec4954 24 Serial sensor(p13,p14);
com3 0:e6d14fec4954 25 Serial pc(USBTX, USBRX);
com3 0:e6d14fec4954 26 TextLCD lcd(p26, p25, p24, p23, p22, p21);
com3 1:fb4277ce4d93 27 AnalogIn adcline[4] = {p16, p17, p19, p20};
com3 1:fb4277ce4d93 28 Timeout liner0;
com3 1:fb4277ce4d93 29 Timeout liner1;
com3 1:fb4277ce4d93 30 Timeout liner2;
com3 1:fb4277ce4d93 31 Timeout liner3;
com3 0:e6d14fec4954 32 //HMC6352 dcompass(p9,p10);
com3 0:e6d14fec4954 33
com3 0:e6d14fec4954 34 extern string StringFIN;
com3 0:e6d14fec4954 35 extern void array(int,int,int,int);
com3 0:e6d14fec4954 36 extern void micon_rx(void);
com3 0:e6d14fec4954 37
com3 0:e6d14fec4954 38 //uint16_t analogHex[4] = {0};
com3 0:e6d14fec4954 39 int speed[4] = {0};
com3 1:fb4277ce4d93 40 uint8_t value_ir = 0, ir_num = 0;
com3 0:e6d14fec4954 41 uint8_t ping[4] = {0};
com3 1:fb4277ce4d93 42 uint8_t line[4] = {0}, line_stop[4] = {0};
com3 1:fb4277ce4d93 43 uint8_t back = 0;
com3 0:e6d14fec4954 44 int compass = 0;
com3 0:e6d14fec4954 45 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
com3 0:e6d14fec4954 46 int compassdef = 0, data = 0;
com3 1:fb4277ce4d93 47 uint8_t pingdef[4] = {0};
com3 1:fb4277ce4d93 48
com3 1:fb4277ce4d93 49 double way[8][2] = {
com3 1:fb4277ce4d93 50 { 0 , 1 },
com3 1:fb4277ce4d93 51 {-0.707, 0.707},
com3 1:fb4277ce4d93 52 {-1 , 0 },
com3 1:fb4277ce4d93 53 {-0.707,-0.707},
com3 1:fb4277ce4d93 54 { 0 ,-1 },
com3 1:fb4277ce4d93 55 { 0.707,-0.707},
com3 1:fb4277ce4d93 56 { 1 , 0 },
com3 1:fb4277ce4d93 57 { 0.707, 0.707}
com3 1:fb4277ce4d93 58 };
com3 0:e6d14fec4954 59
com3 0:e6d14fec4954 60
com3 0:e6d14fec4954 61 void move(int vx, int vy, int vs){
com3 0:e6d14fec4954 62 double pwm[4] = {0};
com3 0:e6d14fec4954 63
com3 0:e6d14fec4954 64 pwm[0] = (double)((vx) + vs);
com3 0:e6d14fec4954 65 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 66 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 67 pwm[3] = 0;
com3 0:e6d14fec4954 68
com3 0:e6d14fec4954 69 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 70 if(pwm[i] > 100){
com3 0:e6d14fec4954 71 pwm[i] = 100;
com3 0:e6d14fec4954 72 } else if(pwm[i] < -100){
com3 0:e6d14fec4954 73 pwm[i] = -100;
com3 0:e6d14fec4954 74 }
com3 0:e6d14fec4954 75 speed[i] = pwm[i];
com3 0:e6d14fec4954 76 }
com3 0:e6d14fec4954 77 }
com3 0:e6d14fec4954 78
com3 0:e6d14fec4954 79 //通信（モータ用）
com3 0:e6d14fec4954 80 void tx_motor(){
com3 0:e6d14fec4954 81 array(speed[0],speed[1],speed[3],speed[2]);
com3 0:e6d14fec4954 82 motor.printf("%s",StringFIN.c_str());
com3 0:e6d14fec4954 83 }
com3 0:e6d14fec4954 84
com3 0:e6d14fec4954 85 //ライン判断
com3 0:e6d14fec4954 86 void line_state(){
com3 0:e6d14fec4954 87 if(line[0]){
com3 1:fb4277ce4d93 88 y = -LINE_FP;
com3 0:e6d14fec4954 89 }
com3 0:e6d14fec4954 90 if(line[1]){
com3 1:fb4277ce4d93 91 x = LINE_LP;
com3 0:e6d14fec4954 92 }
com3 0:e6d14fec4954 93 if(line[2]){
com3 1:fb4277ce4d93 94 y = LINE_FP;
com3 0:e6d14fec4954 95 }
com3 0:e6d14fec4954 96 if(line[3]){
com3 1:fb4277ce4d93 97 x = -LINE_LP;
com3 0:e6d14fec4954 98 }
com3 0:e6d14fec4954 99 }
com3 0:e6d14fec4954 100
com3 1:fb4277ce4d93 101 void lcds(int lcd1){
com3 1:fb4277ce4d93 102 lcd.cls();
com3 1:fb4277ce4d93 103 lcd.locate(0, 0);
com3 1:fb4277ce4d93 104 lcd.printf("%d", lcd1);
com3 1:fb4277ce4d93 105 //lcd.locate(2, 1);
com3 1:fb4277ce4d93 106 //lcd.printf("YATTIYATTI");
com3 1:fb4277ce4d93 107 }
com3 1:fb4277ce4d93 108
com3 1:fb4277ce4d93 109 void lcd_ping(){
com3 1:fb4277ce4d93 110 lcd.cls();
com3 1:fb4277ce4d93 111 lcd.locate(0,0);
com3 1:fb4277ce4d93 112 lcd.printf("%03d %03d\n%03d %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
com3 1:fb4277ce4d93 113 }
com3 1:fb4277ce4d93 114
com3 1:fb4277ce4d93 115 void lcd_line(){
com3 1:fb4277ce4d93 116 lcd.cls();
com3 1:fb4277ce4d93 117 lcd.locate(0,0);
com3 1:fb4277ce4d93 118 lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
com3 1:fb4277ce4d93 119 }
com3 1:fb4277ce4d93 120
com3 1:fb4277ce4d93 121
com3 1:fb4277ce4d93 122 void home(){
com3 1:fb4277ce4d93 123 if(ping[LEFT] > 80){
com3 1:fb4277ce4d93 124 x = -20;
com3 1:fb4277ce4d93 125 } else if(ping[RIGHT] > 80){
com3 1:fb4277ce4d93 126 x = 20;
com3 1:fb4277ce4d93 127 }
com3 1:fb4277ce4d93 128 back = 30;
com3 1:fb4277ce4d93 129 /* if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){
com3 1:fb4277ce4d93 130 back = 10;
com3 1:fb4277ce4d93 131 }*/
com3 1:fb4277ce4d93 132 if(ping[BACK] > back){
com3 1:fb4277ce4d93 133 y = -20;
com3 1:fb4277ce4d93 134 }
com3 1:fb4277ce4d93 135 }
com3 1:fb4277ce4d93 136
com3 1:fb4277ce4d93 137 void line_stop0(){
com3 1:fb4277ce4d93 138 line_stop[0] = 0;
com3 1:fb4277ce4d93 139 }
com3 1:fb4277ce4d93 140 void line_stop1(){
com3 1:fb4277ce4d93 141 line_stop[1] = 0;
com3 1:fb4277ce4d93 142 }
com3 1:fb4277ce4d93 143 void line_stop2(){
com3 1:fb4277ce4d93 144 line_stop[2] = 0;
com3 1:fb4277ce4d93 145 }
com3 1:fb4277ce4d93 146 void line_stop3(){
com3 1:fb4277ce4d93 147 line_stop[3] = 0;
com3 1:fb4277ce4d93 148 }
com3 1:fb4277ce4d93 149
com3 0:e6d14fec4954 150 int main() {
com3 0:e6d14fec4954 151
com3 0:e6d14fec4954 152 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
com3 1:fb4277ce4d93 153 uint8_t num = 0;
com3 0:e6d14fec4954 154
com3 0:e6d14fec4954 155 wait(1);
com3 0:e6d14fec4954 156
com3 0:e6d14fec4954 157 motor.baud(115200); //ボーレート設定
com3 0:e6d14fec4954 158 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
com3 0:e6d14fec4954 159 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
com3 0:e6d14fec4954 160 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
com3 0:e6d14fec4954 161 //compassdef = (compass / 10); //コンパス初期値保存
com3 0:e6d14fec4954 162 //compassdef = (dcompass.sample() / 10);
com3 0:e6d14fec4954 163
com3 1:fb4277ce4d93 164 sw.mode(PullUp);
com3 1:fb4277ce4d93 165 start.mode(PullUp);
com3 0:e6d14fec4954 166
com3 1:fb4277ce4d93 167 myled[0] = 1;
com3 1:fb4277ce4d93 168 while(start){}
com3 1:fb4277ce4d93 169 myled[0] = 0;
com3 1:fb4277ce4d93 170 /*
com3 1:fb4277ce4d93 171 while(1){
com3 1:fb4277ce4d93 172 //x = 30;
com3 1:fb4277ce4d93 173 //y = 35;
com3 1:fb4277ce4d93 174 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 175 if(s > S1){
com3 1:fb4277ce4d93 176 s = S1;
com3 1:fb4277ce4d93 177 } else if(s < -S1){
com3 1:fb4277ce4d93 178 s = -S1;
com3 1:fb4277ce4d93 179 } else if(s > S2){
com3 1:fb4277ce4d93 180 s = S2;
com3 1:fb4277ce4d93 181 } else if(s < -S2){
com3 1:fb4277ce4d93 182 s = -S2;
com3 1:fb4277ce4d93 183 } else if(s > S3){
com3 1:fb4277ce4d93 184 s = S3;
com3 1:fb4277ce4d93 185 } else if(s < -S3){
com3 1:fb4277ce4d93 186 s = -S3;
com3 1:fb4277ce4d93 187 }
com3 1:fb4277ce4d93 188 move(x,y,s);
com3 1:fb4277ce4d93 189 //pc.printf("%d\n", s);
com3 1:fb4277ce4d93 190 //pc.printf("%d\n", adcline[3].read_u16());
com3 1:fb4277ce4d93 191 }
com3 1:fb4277ce4d93 192 */
com3 1:fb4277ce4d93 193 /* while(1){
com3 1:fb4277ce4d93 194 //lcd_ping();
com3 1:fb4277ce4d93 195 lcd_line();
com3 1:fb4277ce4d93 196 wait(0.2);
com3 1:fb4277ce4d93 197 }
com3 1:fb4277ce4d93 198 */
com3 1:fb4277ce4d93 199 /* while(1){
com3 1:fb4277ce4d93 200 if(ir_num > 7){
com3 1:fb4277ce4d93 201 ir_num = 0;
com3 1:fb4277ce4d93 202 }
com3 1:fb4277ce4d93 203 x = MOTOR_P * way[ir_num][0];
com3 1:fb4277ce4d93 204 y = MOTOR_P * way[ir_num][1];
com3 1:fb4277ce4d93 205 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 206 if(s > S_MAX){
com3 1:fb4277ce4d93 207 s = S_MAX;
com3 1:fb4277ce4d93 208 } else if(s < -S_MAX){
com3 1:fb4277ce4d93 209 s = -S_MAX;
com3 1:fb4277ce4d93 210 }
com3 1:fb4277ce4d93 211
com3 1:fb4277ce4d93 212 move(x,y,s);
com3 1:fb4277ce4d93 213 }
com3 1:fb4277ce4d93 214 */
com3 1:fb4277ce4d93 215 while(1){
com3 1:fb4277ce4d93 216 if(!line_stop[0]){
com3 1:fb4277ce4d93 217 if(adcline[0].read_u16() > LINE_ON){
com3 1:fb4277ce4d93 218 line[0] = 1;
com3 1:fb4277ce4d93 219 line_stop[0] = 1;
com3 1:fb4277ce4d93 220 myled[0] = 1;
com3 1:fb4277ce4d93 221 liner0.attach(&line_stop0,LINE_TIME);
com3 1:fb4277ce4d93 222 } else {
com3 1:fb4277ce4d93 223 line[0] = 0;
com3 1:fb4277ce4d93 224 myled[0] = 0;
com3 1:fb4277ce4d93 225 }
com3 1:fb4277ce4d93 226 }
com3 1:fb4277ce4d93 227 if(!line_stop[1]){
com3 1:fb4277ce4d93 228 if(adcline[1].read_u16() > LINE_ON){
com3 1:fb4277ce4d93 229 line[1] = 1;
com3 1:fb4277ce4d93 230 line_stop[1] = 1;
com3 1:fb4277ce4d93 231 myled[1] = 1;
com3 1:fb4277ce4d93 232 liner1.attach(&line_stop1,LINE_TIME);
com3 1:fb4277ce4d93 233 } else {
com3 1:fb4277ce4d93 234 line[1] = 0;
com3 1:fb4277ce4d93 235 myled[1] = 0;
com3 1:fb4277ce4d93 236 }
com3 1:fb4277ce4d93 237 }
com3 1:fb4277ce4d93 238 if(!line_stop[2]){
com3 1:fb4277ce4d93 239 if(adcline[2].read_u16() > LINE_ON){
com3 1:fb4277ce4d93 240 line[2] = 1;
com3 1:fb4277ce4d93 241 line_stop[2] = 1;
com3 1:fb4277ce4d93 242 myled[2] = 1;
com3 1:fb4277ce4d93 243 liner2.attach(&line_stop2,LINE_TIME);
com3 1:fb4277ce4d93 244 } else {
com3 1:fb4277ce4d93 245 line[2] = 0;
com3 1:fb4277ce4d93 246 myled[2] = 0;
com3 1:fb4277ce4d93 247 }
com3 1:fb4277ce4d93 248 }
com3 1:fb4277ce4d93 249 if(!line_stop[3]){
com3 1:fb4277ce4d93 250 if(adcline[3].read_u16() > LINE_ON){
com3 1:fb4277ce4d93 251 line[3] = 1;
com3 1:fb4277ce4d93 252 line_stop[3] = 1;
com3 1:fb4277ce4d93 253 myled[3] = 1;
com3 1:fb4277ce4d93 254 liner3.attach(&line_stop3,LINE_TIME);
com3 1:fb4277ce4d93 255 } else {
com3 1:fb4277ce4d93 256 line[3] = 0;
com3 1:fb4277ce4d93 257 myled[3] = 0;
com3 1:fb4277ce4d93 258 }
com3 1:fb4277ce4d93 259 }
com3 1:fb4277ce4d93 260 //pc.printf("%d\n", adcline[1].read_u16());
com3 1:fb4277ce4d93 261 //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
com3 1:fb4277ce4d93 262 if(ir_num > 7){
com3 1:fb4277ce4d93 263 ir_num = 0;
com3 1:fb4277ce4d93 264 }
com3 1:fb4277ce4d93 265 x = 30;//MOTOR_P * way[ir_num][0];
com3 1:fb4277ce4d93 266 y = 0;//MOTOR_P * way[ir_num][1];
com3 1:fb4277ce4d93 267 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 268 if(s > S1){
com3 1:fb4277ce4d93 269 s = S1;
com3 1:fb4277ce4d93 270 } else if(s < -S1){
com3 1:fb4277ce4d93 271 s = -S1;
com3 1:fb4277ce4d93 272 } else if(s > S2){
com3 1:fb4277ce4d93 273 s = S2;
com3 1:fb4277ce4d93 274 } else if(s < -S2){
com3 1:fb4277ce4d93 275 s = -S2;
com3 1:fb4277ce4d93 276 } else if(s > S3){
com3 1:fb4277ce4d93 277 s = S3;
com3 1:fb4277ce4d93 278 } else if(s < -S3){
com3 1:fb4277ce4d93 279 s = -S3;
com3 1:fb4277ce4d93 280 }
com3 1:fb4277ce4d93 281 line_state();
com3 1:fb4277ce4d93 282 move(x,y,s);
com3 1:fb4277ce4d93 283 }
com3 1:fb4277ce4d93 284 //y = 20;
com3 0:e6d14fec4954 285
com3 0:e6d14fec4954 286 while(1){
com3 1:fb4277ce4d93 287 x = 20 * way[num][0];
com3 1:fb4277ce4d93 288 y = 20 * way[num][1];
com3 1:fb4277ce4d93 289 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 290
com3 1:fb4277ce4d93 291 if(!sw){
com3 1:fb4277ce4d93 292 num++;
com3 1:fb4277ce4d93 293 wait(0.2);
com3 1:fb4277ce4d93 294 if(num > 7){
com3 1:fb4277ce4d93 295 num = 0;
com3 1:fb4277ce4d93 296 }
com3 1:fb4277ce4d93 297 lcds(num);
com3 1:fb4277ce4d93 298 }
com3 0:e6d14fec4954 299 //pc.printf("%d\n", s);
com3 0:e6d14fec4954 300
com3 0:e6d14fec4954 301 move(x,y,s);
com3 0:e6d14fec4954 302 //wait(0.1);
com3 0:e6d14fec4954 303 }
com3 0:e6d14fec4954 304 /*
com3 0:e6d14fec4954 305 while(1){
com3 0:e6d14fec4954 306 i = 3;
com3 0:e6d14fec4954 307 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 308 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 309 myled[i] = 1;
com3 0:e6d14fec4954 310 line[i] = 1;
com3 0:e6d14fec4954 311 line_on = 1;
com3 0:e6d14fec4954 312 } else {
com3 0:e6d14fec4954 313 myled[i] = 0;
com3 0:e6d14fec4954 314 line[i] = 0;
com3 0:e6d14fec4954 315 line_on = 0;
com3 0:e6d14fec4954 316 }
com3 0:e6d14fec4954 317
com3 0:e6d14fec4954 318 x = 30;
com3 0:e6d14fec4954 319 y = 0;
com3 0:e6d14fec4954 320
com3 0:e6d14fec4954 321 if(line[1]){
com3 0:e6d14fec4954 322 x = LINE_P;
com3 0:e6d14fec4954 323 } else if(line[3]){
com3 0:e6d14fec4954 324 x = -LINE_P;
com3 0:e6d14fec4954 325 }
com3 0:e6d14fec4954 326
com3 0:e6d14fec4954 327 if(line[0]){
com3 0:e6d14fec4954 328 y = -LINE_P;
com3 0:e6d14fec4954 329 } else if(line[2]){
com3 0:e6d14fec4954 330 y = LINE_P;
com3 0:e6d14fec4954 331 }
com3 0:e6d14fec4954 332 move(x,y,s);
com3 0:e6d14fec4954 333
com3 0:e6d14fec4954 334 if(line_on)wait(0.3);
com3 0:e6d14fec4954 335 }
com3 0:e6d14fec4954 336 */
com3 0:e6d14fec4954 337 /*
com3 0:e6d14fec4954 338 while(1){
com3 0:e6d14fec4954 339 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 340 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 341 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 342 myled[i] = 1;
com3 0:e6d14fec4954 343 line[i] = 1;
com3 0:e6d14fec4954 344 } else {
com3 0:e6d14fec4954 345 myled[i] = 0;
com3 0:e6d14fec4954 346 line[i] = 0;
com3 0:e6d14fec4954 347 }
com3 0:e6d14fec4954 348 //wait(0.1);
com3 0:e6d14fec4954 349 }
com3 0:e6d14fec4954 350
com3 0:e6d14fec4954 351 if(line[0]){
com3 0:e6d14fec4954 352 x = -30;
com3 0:e6d14fec4954 353 } else if(line[2]){
com3 0:e6d14fec4954 354 x = 30;
com3 0:e6d14fec4954 355 }
com3 0:e6d14fec4954 356
com3 0:e6d14fec4954 357 if(line[1]){
com3 0:e6d14fec4954 358 y = 30;
com3 0:e6d14fec4954 359 } else if(line[3]){
com3 0:e6d14fec4954 360 y = -30;
com3 0:e6d14fec4954 361 }
com3 0:e6d14fec4954 362
com3 0:e6d14fec4954 363 move(x,y);
com3 0:e6d14fec4954 364 x = 0;
com3 0:e6d14fec4954 365 y = 0;
com3 0:e6d14fec4954 366 //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]);
com3 0:e6d14fec4954 367 }
com3 0:e6d14fec4954 368 */
com3 0:e6d14fec4954 369 }