Edwin Cho / Mbed 2 deprecated DEMO3

Localization

Dependencies:   BNO055_fusion mbed

main.cpp@14:4839989ae907, 2016-03-24 (annotated)

Committer:
12104404
Date:
Thu Mar 24 03:54:29 2016 +0000
Revision:
14:4839989ae907
Parent:
13:c62f975dfcfe
Child:
15:7729da55873a
Before I make it a library

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 14:4839989ae907 6 #define SPEED_TURN_MIN 0.15
12104404 14:4839989ae907 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 DigitalOut dir1(p15);
12104404 6:0602a9e8118b 22 DigitalOut dir2(p16);
12104404 6:0602a9e8118b 23
12104404 6:0602a9e8118b 24 PwmOut motor1F(p21);
12104404 6:0602a9e8118b 25 PwmOut motor1B(p22);
12104404 6:0602a9e8118b 26 PwmOut motor2F(p23);
12104404 6:0602a9e8118b 27 PwmOut motor2B(p24);
12104404 6:0602a9e8118b 28
12104404 6:0602a9e8118b 29 Ticker t;
12104404 6:0602a9e8118b 30 bool flag=false;
12104404 6:0602a9e8118b 31
12104404 6:0602a9e8118b 32 void send();
12104404 10:cf44f4387bc7 33 void setAngle(int angle);
12104404 10:cf44f4387bc7 34 int wrap(int a);
12104404 6:0602a9e8118b 35
12104404 0:96d6eb224379 36 int main()
12104404 0:96d6eb224379 37 {
12104404 6:0602a9e8118b 38 wdt.kick(5);
12104404 0:96d6eb224379 39 pc.baud(9600);
12104404 6:0602a9e8118b 40 dir1=0;
12104404 6:0602a9e8118b 41 dir2=0;
12104404 6:0602a9e8118b 42 motor1F=0;
12104404 6:0602a9e8118b 43 motor1B=0;
12104404 6:0602a9e8118b 44 motor2F=0;
12104404 6:0602a9e8118b 45 motor2B=0;
12104404 6:0602a9e8118b 46 //pc.printf("Initialized Localization: %d\n",loc.init());
12104404 8:b36be08c44f8 47 t.attach(&send,1);
12104404 0:96d6eb224379 48 while(1) {
12104404 7:d6dca30f7959 49 //loc.get_angle(&xya);
12104404 8:b36be08c44f8 50 loc.get_xy(&xya);
12104404 8:b36be08c44f8 51 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 14:4839989ae907 52 setAngle(0);
12104404 6:0602a9e8118b 53 wdt.kick();
12104404 0:96d6eb224379 54 }
12104404 0:96d6eb224379 55 }
12104404 6:0602a9e8118b 56
12104404 6:0602a9e8118b 57 void send()
12104404 6:0602a9e8118b 58 {
12104404 8:b36be08c44f8 59 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 60 }
12104404 6:0602a9e8118b 61
12104404 10:cf44f4387bc7 62 void setAngle(int angle)
12104404 6:0602a9e8118b 63 {
12104404 6:0602a9e8118b 64 float s = 0;
12104404 10:cf44f4387bc7 65 int diff = 0;
12104404 10:cf44f4387bc7 66 diff = 180-wrap(angle);
12104404 11:9518f8285906 67 if(abs(wrap(xya.a+diff)-180)<=5)
12104404 10:cf44f4387bc7 68 s=SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 69 else
12104404 12:3b26fcc7da7e 70 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 71 motor1F=s;
12104404 10:cf44f4387bc7 72 motor1B=s;
12104404 10:cf44f4387bc7 73 motor2F=s;
12104404 10:cf44f4387bc7 74 motor2B=s;
12104404 10:cf44f4387bc7 75 if(wrap(xya.a+diff)>180+2) {
12104404 10:cf44f4387bc7 76 dir1=1;
12104404 10:cf44f4387bc7 77 dir2=0;
12104404 10:cf44f4387bc7 78 } else if(wrap(xya.a+diff)<180-2) {
12104404 10:cf44f4387bc7 79 dir1=0;
12104404 10:cf44f4387bc7 80 dir2=1;
12104404 10:cf44f4387bc7 81 } else {
12104404 10:cf44f4387bc7 82 motor1F=0;
12104404 10:cf44f4387bc7 83 motor1B=0;
12104404 10:cf44f4387bc7 84 motor2F=0;
12104404 10:cf44f4387bc7 85 motor2B=0;
12104404 6:0602a9e8118b 86 }
12104404 10:cf44f4387bc7 87 }
12104404 10:cf44f4387bc7 88
12104404 10:cf44f4387bc7 89 int wrap(int a)
12104404 10:cf44f4387bc7 90 {
12104404 10:cf44f4387bc7 91 return a%360;
12104404 10:cf44f4387bc7 92 }