Antoine Laurens / Mbed 2 deprecated STRAIGHT_DRIVE_NO_SEP

drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

main.cpp@16:d6f15a13c3aa, 2016-03-27 (annotated)

Committer:
12104404
Date:
Sun Mar 27 02:39:07 2016 +0000
Revision:
16:d6f15a13c3aa
Parent:
15:7729da55873a
Child:
17:2f89826b5679
ANGLE PORTED and tested

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 6:0602a9e8118b 20 bool flag=false;
12104404 6:0602a9e8118b 21
12104404 6:0602a9e8118b 22 void send();
12104404 15:7729da55873a 23 //void setAngle(int angle);
12104404 10:cf44f4387bc7 24 int wrap(int a);
12104404 6:0602a9e8118b 25
12104404 0:96d6eb224379 26 int main()
12104404 0:96d6eb224379 27 {
12104404 6:0602a9e8118b 28 wdt.kick(5);
12104404 0:96d6eb224379 29 pc.baud(9600);
12104404 6:0602a9e8118b 30 //pc.printf("Initialized Localization: %d\n",loc.init());
12104404 8:b36be08c44f8 31 t.attach(&send,1);
12104404 0:96d6eb224379 32 while(1) {
12104404 7:d6dca30f7959 33 //loc.get_angle(&xya);
12104404 8:b36be08c44f8 34 loc.get_xy(&xya);
12104404 16:d6f15a13c3aa 35 motion.setAngle(0,xya.a,2,ANGLE_TURN);
12104404 8:b36be08c44f8 36 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 6:0602a9e8118b 37 wdt.kick();
12104404 0:96d6eb224379 38 }
12104404 0:96d6eb224379 39 }
12104404 6:0602a9e8118b 40
12104404 6:0602a9e8118b 41 void send()
12104404 6:0602a9e8118b 42 {
12104404 8:b36be08c44f8 43 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 44 }
12104404 15:7729da55873a 45 /*
12104404 10:cf44f4387bc7 46 void setAngle(int angle)
12104404 6:0602a9e8118b 47 {
12104404 6:0602a9e8118b 48 float s = 0;
12104404 10:cf44f4387bc7 49 int diff = 0;
12104404 10:cf44f4387bc7 50 diff = 180-wrap(angle);
12104404 11:9518f8285906 51 if(abs(wrap(xya.a+diff)-180)<=5)
12104404 10:cf44f4387bc7 52 s=SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 53 else
12104404 12:3b26fcc7da7e 54 s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
12104404 10:cf44f4387bc7 55 motor1F=s;
12104404 10:cf44f4387bc7 56 motor1B=s;
12104404 10:cf44f4387bc7 57 motor2F=s;
12104404 10:cf44f4387bc7 58 motor2B=s;
12104404 10:cf44f4387bc7 59 if(wrap(xya.a+diff)>180+2) {
12104404 10:cf44f4387bc7 60 dir1=1;
12104404 10:cf44f4387bc7 61 dir2=0;
12104404 10:cf44f4387bc7 62 } else if(wrap(xya.a+diff)<180-2) {
12104404 10:cf44f4387bc7 63 dir1=0;
12104404 10:cf44f4387bc7 64 dir2=1;
12104404 10:cf44f4387bc7 65 } else {
12104404 10:cf44f4387bc7 66 motor1F=0;
12104404 10:cf44f4387bc7 67 motor1B=0;
12104404 10:cf44f4387bc7 68 motor2F=0;
12104404 10:cf44f4387bc7 69 motor2B=0;
12104404 6:0602a9e8118b 70 }
12104404 15:7729da55873a 71 }*/
12104404 10:cf44f4387bc7 72
12104404 10:cf44f4387bc7 73 int wrap(int a)
12104404 10:cf44f4387bc7 74 {
12104404 10:cf44f4387bc7 75 return a%360;
12104404 10:cf44f4387bc7 76 }