Antoine Laurens / Mbed 2 deprecated DEMO22

version2

Dependencies:   BNO055_fusion mbed

Fork of DEMO2 by Antoine Laurens

main.cpp@20:1da786e205eb, 2016-03-29 (annotated)

Committer:
alaurens
Date:
Tue Mar 29 22:49:05 2016 +0000
Revision:
20:1da786e205eb
Parent:
19:5832e34b7533 
should theoretically work

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 6:0602a9e8118b 5 #define SPEED_FB_MIN 0.15
12104404 6:0602a9e8118b 6 #define SPEED_FB_MAX 0.50
12104404 6:0602a9e8118b 7
12104404 8:b36be08c44f8 8 Serial pc(p13, p14);
12104404 8:b36be08c44f8 9 //Serial pc(USBTX, USBRX);
12104404 6:0602a9e8118b 10
12104404 6:0602a9e8118b 11 Watchdog wdt;
12104404 0:96d6eb224379 12
12104404 0:96d6eb224379 13 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 14 I2C i2c2(p9, p10);
12104404 16:d6f15a13c3aa 15 LOCALIZE loc(i2c1, i2c2, p26, p8, p7, p6, p5);
12104404 0:96d6eb224379 16 LOCALIZE_xya xya;
12104404 15:7729da55873a 17 LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
12104404 6:0602a9e8118b 18
12104404 6:0602a9e8118b 19 Ticker t;
12104404 17:2f89826b5679 20 Ticker tTarget;
12104404 6:0602a9e8118b 21 bool flag=false;
alaurens 20:1da786e205eb 22 int xTarget=20;
12104404 17:2f89826b5679 23 int angle_error=2;
12104404 17:2f89826b5679 24 bool xGood=false;
alaurens 19:5832e34b7533 25 bool yGood=false;
alaurens 19:5832e34b7533 26 bool angleGood=false;
alaurens 20:1da786e205eb 27 int xState=X_INCREASE;
12104404 18:f9012e93edb8 28 int angleTarget=0;
alaurens 19:5832e34b7533 29 int yTarget=30;
alaurens 20:1da786e205eb 30 //void setTarget();
12104404 6:0602a9e8118b 31 void send();
12104404 15:7729da55873a 32 //void setAngle(int angle);
12104404 10:cf44f4387bc7 33 int wrap(int a);
12104404 6:0602a9e8118b 34
12104404 0:96d6eb224379 35 int main()
12104404 0:96d6eb224379 36 {
12104404 6:0602a9e8118b 37 wdt.kick(5);
12104404 0:96d6eb224379 38 pc.baud(9600);
12104404 6:0602a9e8118b 39 //pc.printf("Initialized Localization: %d\n",loc.init());
12104404 8:b36be08c44f8 40 t.attach(&send,1);
12104404 18:f9012e93edb8 41 //tTarget.attach(&setTarget,7);
12104404 0:96d6eb224379 42 while(1) {
12104404 7:d6dca30f7959 43 //loc.get_angle(&xya);
12104404 8:b36be08c44f8 44 loc.get_xy(&xya);
alaurens 20:1da786e205eb 45 motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,angle_error);
alaurens 20:1da786e205eb 46
alaurens 20:1da786e205eb 47 motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
alaurens 20:1da786e205eb 48 motion.setAngleTol(&angle_error,yGood,xGood);
alaurens 20:1da786e205eb 49 motion.setYgoal(xGood,angleGood,yGood,&yTarget);
12104404 18:f9012e93edb8 50 if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
alaurens 20:1da786e205eb 51 motion.setXPos(xTarget,xya.x,2,angleTarget);
alaurens 20:1da786e205eb 52 motion.setYPos(yTarget,xya.y,2,angleTarget);
alaurens 20:1da786e205eb 53
12104404 17:2f89826b5679 54 }
alaurens 20:1da786e205eb 55
12104404 8:b36be08c44f8 56 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 6:0602a9e8118b 57 wdt.kick();
12104404 0:96d6eb224379 58 }
12104404 0:96d6eb224379 59 }
12104404 6:0602a9e8118b 60
alaurens 20:1da786e205eb 61
12104404 6:0602a9e8118b 62 void send()
12104404 6:0602a9e8118b 63 {
12104404 8:b36be08c44f8 64 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 65 }
12104404 17:2f89826b5679 66
12104404 17:2f89826b5679 67 void setTarget()
12104404 6:0602a9e8118b 68 {
alaurens 20:1da786e205eb 69 xTarget=xTarget==20?80:20;
12104404 17:2f89826b5679 70 }