Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

main.cpp@12:3b26fcc7da7e, 2016-03-23 (annotated)

Committer:
12104404
Date:
Wed Mar 23 16:25:48 2016 +0000
Revision:
12:3b26fcc7da7e
Parent:
11:9518f8285906
Child:
13:c62f975dfcfe
fixed speed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
12104404 0:96d6eb224379 1 #include "LOCALIZE.h"
12104404 6:0602a9e8118b 2 #include "LOCOMOTION.h"
12104404 6:0602a9e8118b 3 #include "WATCHDOG.h"
12104404 0:96d6eb224379 4
12104404 10:cf44f4387bc7 5
12104404 10:cf44f4387bc7 6 #define SPEED_TURN_MIN 0.20
12104404 6:0602a9e8118b 7 #define SPEED_TURN_MAX 0.35
12104404 6:0602a9e8118b 8 #define SPEED_FB_MIN 0.15
12104404 6:0602a9e8118b 9 #define SPEED_FB_MAX 0.50
12104404 6:0602a9e8118b 10
12104404 8:b36be08c44f8 11 Serial pc(p13, p14);
12104404 8:b36be08c44f8 12 //Serial pc(USBTX, USBRX);
12104404 6:0602a9e8118b 13
12104404 6:0602a9e8118b 14 Watchdog wdt;
12104404 0:96d6eb224379 15
12104404 0:96d6eb224379 16 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 17 I2C i2c2(p9, p10);
12104404 0:96d6eb224379 18 LOCALIZE loc(i2c1, i2c2, p26);
12104404 0:96d6eb224379 19 LOCALIZE_xya xya;
12104404 0:96d6eb224379 20
12104404 6:0602a9e8118b 21 DigitalIn sw1(p20);
12104404 6:0602a9e8118b 22 DigitalIn sw2(p19);
12104404 6:0602a9e8118b 23 DigitalIn sw3(p18);
12104404 6:0602a9e8118b 24 DigitalIn sw4(p17);
12104404 6:0602a9e8118b 25 DigitalOut dir1(p15);
12104404 6:0602a9e8118b 26 DigitalOut dir2(p16);
12104404 6:0602a9e8118b 27
12104404 6:0602a9e8118b 28 PwmOut motor1F(p21);
12104404 6:0602a9e8118b 29 PwmOut motor1B(p22);
12104404 6:0602a9e8118b 30 PwmOut motor2F(p23);
12104404 6:0602a9e8118b 31 PwmOut motor2B(p24);
12104404 6:0602a9e8118b 32
12104404 0:96d6eb224379 33 DigitalOut led1(LED1);
12104404 0:96d6eb224379 34 DigitalOut led2(LED2);
12104404 0:96d6eb224379 35 DigitalOut led3(LED3);
12104404 0:96d6eb224379 36 DigitalOut led4(LED4);
12104404 0:96d6eb224379 37
12104404 6:0602a9e8118b 38 Ticker t;
12104404 6:0602a9e8118b 39 bool flag=false;
12104404 6:0602a9e8118b 40
12104404 6:0602a9e8118b 41 void send();
12104404 10:cf44f4387bc7 42 void setAngle(int angle);
12104404 10:cf44f4387bc7 43 int wrap(int a);
12104404 6:0602a9e8118b 44
12104404 0:96d6eb224379 45 int main()
12104404 0:96d6eb224379 46 {
12104404 6:0602a9e8118b 47 wdt.kick(5);
12104404 0:96d6eb224379 48 pc.baud(9600);
12104404 6:0602a9e8118b 49 dir1=0;
12104404 6:0602a9e8118b 50 dir2=0;
12104404 6:0602a9e8118b 51 motor1F=0;
12104404 6:0602a9e8118b 52 motor1B=0;
12104404 6:0602a9e8118b 53 motor2F=0;
12104404 6:0602a9e8118b 54 motor2B=0;
12104404 6:0602a9e8118b 55 //pc.printf("Initialized Localization: %d\n",loc.init());
12104404 8:b36be08c44f8 56 t.attach(&send,1);
12104404 6:0602a9e8118b 57 sw1.mode(PullUp);
12104404 6:0602a9e8118b 58 sw2.mode(PullUp);
12104404 6:0602a9e8118b 59 sw3.mode(PullUp);
12104404 6:0602a9e8118b 60 sw4.mode(PullUp);
12104404 0:96d6eb224379 61 while(1) {
12104404 6:0602a9e8118b 62 led1=!sw1;
12104404 6:0602a9e8118b 63 led2=!sw2;
12104404 6:0602a9e8118b 64 led3=!sw3;
12104404 6:0602a9e8118b 65 led4=!sw4;
12104404 7:d6dca30f7959 66 //loc.get_angle(&xya);
12104404 8:b36be08c44f8 67 loc.get_xy(&xya);
12104404 8:b36be08c44f8 68 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 12:3b26fcc7da7e 69 setAngle(170);
12104404 6:0602a9e8118b 70 wdt.kick();
12104404 0:96d6eb224379 71 }
12104404 0:96d6eb224379 72 }
12104404 6:0602a9e8118b 73
12104404 6:0602a9e8118b 74 void send()
12104404 6:0602a9e8118b 75 {
12104404 8:b36be08c44f8 76 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 77 }
12104404 6:0602a9e8118b 78
12104404 10:cf44f4387bc7 79 void setAngle(int angle)
12104404 6:0602a9e8118b 80 {
12104404 6:0602a9e8118b 81 float s = 0;
12104404 10:cf44f4387bc7 82 int diff = 0;
12104404 10:cf44f4387bc7 83 diff = 180-wrap(angle);
12104404 11:9518f8285906 84 if(abs(wrap(xya.a+diff)-180)<=5)
12104404 10:cf44f4387bc7 85 s=SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 86 else
12104404 12:3b26fcc7da7e 87 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 88 motor1F=s;
12104404 10:cf44f4387bc7 89 motor1B=s;
12104404 10:cf44f4387bc7 90 motor2F=s;
12104404 10:cf44f4387bc7 91 motor2B=s;
12104404 10:cf44f4387bc7 92 if(wrap(xya.a+diff)>180+2) {
12104404 10:cf44f4387bc7 93 dir1=1;
12104404 10:cf44f4387bc7 94 dir2=0;
12104404 10:cf44f4387bc7 95 } else if(wrap(xya.a+diff)<180-2) {
12104404 10:cf44f4387bc7 96 dir1=0;
12104404 10:cf44f4387bc7 97 dir2=1;
12104404 10:cf44f4387bc7 98 } else {
12104404 10:cf44f4387bc7 99 motor1F=0;
12104404 10:cf44f4387bc7 100 motor1B=0;
12104404 10:cf44f4387bc7 101 motor2F=0;
12104404 10:cf44f4387bc7 102 motor2B=0;
12104404 6:0602a9e8118b 103 }
12104404 10:cf44f4387bc7 104 }
12104404 10:cf44f4387bc7 105
12104404 10:cf44f4387bc7 106 int wrap(int a)
12104404 10:cf44f4387bc7 107 {
12104404 10:cf44f4387bc7 108 return a%360;
12104404 10:cf44f4387bc7 109 }