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@8:b36be08c44f8, 2016-03-23 (annotated)

Committer:
12104404
Date:
Wed Mar 23 06:55:39 2016 +0000
Revision:
8:b36be08c44f8
Parent:
7:d6dca30f7959
Child:
9:4fc5d70c3bab
frame

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_TURN_MIN 0.15
12104404 6:0602a9e8118b 6 #define SPEED_TURN_MAX 0.35
12104404 6:0602a9e8118b 7 #define SPEED_FB_MIN 0.15
12104404 6:0602a9e8118b 8 #define SPEED_FB_MAX 0.50
12104404 6:0602a9e8118b 9
12104404 8:b36be08c44f8 10 Serial pc(p13, p14);
12104404 8:b36be08c44f8 11 //Serial pc(USBTX, USBRX);
12104404 6:0602a9e8118b 12
12104404 6:0602a9e8118b 13 Watchdog wdt;
12104404 0:96d6eb224379 14
12104404 0:96d6eb224379 15 I2C i2c1(p28, p27);
12104404 0:96d6eb224379 16 I2C i2c2(p9, p10);
12104404 0:96d6eb224379 17 LOCALIZE loc(i2c1, i2c2, p26);
12104404 0:96d6eb224379 18 LOCALIZE_xya xya;
12104404 0:96d6eb224379 19
12104404 6:0602a9e8118b 20 DigitalIn sw1(p20);
12104404 6:0602a9e8118b 21 DigitalIn sw2(p19);
12104404 6:0602a9e8118b 22 DigitalIn sw3(p18);
12104404 6:0602a9e8118b 23 DigitalIn sw4(p17);
12104404 6:0602a9e8118b 24 DigitalOut dir1(p15);
12104404 6:0602a9e8118b 25 DigitalOut dir2(p16);
12104404 6:0602a9e8118b 26
12104404 6:0602a9e8118b 27 PwmOut motor1F(p21);
12104404 6:0602a9e8118b 28 PwmOut motor1B(p22);
12104404 6:0602a9e8118b 29 PwmOut motor2F(p23);
12104404 6:0602a9e8118b 30 PwmOut motor2B(p24);
12104404 6:0602a9e8118b 31
12104404 0:96d6eb224379 32 DigitalOut led1(LED1);
12104404 0:96d6eb224379 33 DigitalOut led2(LED2);
12104404 0:96d6eb224379 34 DigitalOut led3(LED3);
12104404 0:96d6eb224379 35 DigitalOut led4(LED4);
12104404 0:96d6eb224379 36
12104404 6:0602a9e8118b 37 Ticker t;
12104404 6:0602a9e8118b 38 bool flag=false;
12104404 6:0602a9e8118b 39
12104404 6:0602a9e8118b 40 void send();
12104404 6:0602a9e8118b 41 void turn(int angle);
12104404 6:0602a9e8118b 42
12104404 0:96d6eb224379 43 int main()
12104404 0:96d6eb224379 44 {
12104404 6:0602a9e8118b 45 wdt.kick(5);
12104404 0:96d6eb224379 46 pc.baud(9600);
12104404 6:0602a9e8118b 47 dir1=0;
12104404 6:0602a9e8118b 48 dir2=0;
12104404 6:0602a9e8118b 49 motor1F=0;
12104404 6:0602a9e8118b 50 motor1B=0;
12104404 6:0602a9e8118b 51 motor2F=0;
12104404 6:0602a9e8118b 52 motor2B=0;
12104404 6:0602a9e8118b 53 //pc.printf("Initialized Localization: %d\n",loc.init());
12104404 8:b36be08c44f8 54 t.attach(&send,1);
12104404 6:0602a9e8118b 55 sw1.mode(PullUp);
12104404 6:0602a9e8118b 56 sw2.mode(PullUp);
12104404 6:0602a9e8118b 57 sw3.mode(PullUp);
12104404 6:0602a9e8118b 58 sw4.mode(PullUp);
12104404 7:d6dca30f7959 59 /*while(abs(xya.a-180)>5) {
12104404 6:0602a9e8118b 60 loc.get_angle(&xya);
12104404 6:0602a9e8118b 61 turn(180);
12104404 7:d6dca30f7959 62 }*/
12104404 0:96d6eb224379 63 while(1) {
12104404 6:0602a9e8118b 64 led1=!sw1;
12104404 6:0602a9e8118b 65 led2=!sw2;
12104404 6:0602a9e8118b 66 led3=!sw3;
12104404 6:0602a9e8118b 67 led4=!sw4;
12104404 7:d6dca30f7959 68 //loc.get_angle(&xya);
12104404 8:b36be08c44f8 69 loc.get_xy(&xya);
12104404 8:b36be08c44f8 70 //loc.get_raw_xy();
12104404 8:b36be08c44f8 71 //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10);
12104404 8:b36be08c44f8 72 //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
12104404 6:0602a9e8118b 73 /*if(loc._rx_n>20) {
12104404 6:0602a9e8118b 74 dir1=1;
12104404 6:0602a9e8118b 75 dir2=1;
12104404 6:0602a9e8118b 76 } else if(loc._rx_n<10) {
12104404 6:0602a9e8118b 77 dir1=0;
12104404 6:0602a9e8118b 78 dir2=0;
12104404 6:0602a9e8118b 79 }*/
12104404 7:d6dca30f7959 80 /*
12104404 6:0602a9e8118b 81 if((!sw1==1 && !sw2==1) && flag) {
12104404 6:0602a9e8118b 82 motor1F=0.3;
12104404 6:0602a9e8118b 83 motor1B=0.3;
12104404 6:0602a9e8118b 84 motor2F=0.3;
12104404 6:0602a9e8118b 85 motor2B=0.3;
12104404 6:0602a9e8118b 86 dir1=0;
12104404 6:0602a9e8118b 87 dir2=0;
12104404 6:0602a9e8118b 88 wait(0.5);
12104404 6:0602a9e8118b 89 while((xya.a>5 && xya.a<355)) {
12104404 6:0602a9e8118b 90 loc.get_angle(&xya);
12104404 6:0602a9e8118b 91 turn(0);
12104404 6:0602a9e8118b 92 }
12104404 6:0602a9e8118b 93 flag=false;
12104404 6:0602a9e8118b 94 } else if((!sw1==1 && !sw2==1) && !flag) {
12104404 6:0602a9e8118b 95 motor1F=0.3;
12104404 6:0602a9e8118b 96 motor1B=0.3;
12104404 6:0602a9e8118b 97 motor2F=0.3;
12104404 6:0602a9e8118b 98 motor2B=0.3;
12104404 6:0602a9e8118b 99 dir1=0;
12104404 6:0602a9e8118b 100 dir2=0;
12104404 6:0602a9e8118b 101 wait(0.5);
12104404 6:0602a9e8118b 102 while(abs(xya.a-180)>=5) {
12104404 6:0602a9e8118b 103 loc.get_angle(&xya);
12104404 6:0602a9e8118b 104 turn(180);
12104404 6:0602a9e8118b 105 }
12104404 6:0602a9e8118b 106 flag=true;
12104404 6:0602a9e8118b 107 } else {
12104404 6:0602a9e8118b 108 float s=0;
12104404 6:0602a9e8118b 109 if(loc._rx_n<=10)
12104404 6:0602a9e8118b 110 s=0.1;
12104404 6:0602a9e8118b 111 else if(loc._rx_n<=30)
12104404 6:0602a9e8118b 112 s=0.4*abs(loc._rx_n-30)/30+0.1;
12104404 6:0602a9e8118b 113 else
12104404 6:0602a9e8118b 114 s=0.5;
12104404 6:0602a9e8118b 115 motor1F=s;
12104404 6:0602a9e8118b 116 motor1B=s;
12104404 6:0602a9e8118b 117 motor2F=s;
12104404 6:0602a9e8118b 118 motor2B=s;
12104404 6:0602a9e8118b 119 dir1=1;
12104404 6:0602a9e8118b 120 dir2=1;
12104404 6:0602a9e8118b 121 }
12104404 7:d6dca30f7959 122 */
12104404 6:0602a9e8118b 123 wdt.kick();
12104404 0:96d6eb224379 124 }
12104404 0:96d6eb224379 125 }
12104404 6:0602a9e8118b 126
12104404 6:0602a9e8118b 127 void send()
12104404 6:0602a9e8118b 128 {
12104404 8:b36be08c44f8 129 pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
12104404 6:0602a9e8118b 130 }
12104404 6:0602a9e8118b 131
12104404 6:0602a9e8118b 132 void turn(int angle)
12104404 6:0602a9e8118b 133 {
12104404 6:0602a9e8118b 134 float s = 0;
12104404 6:0602a9e8118b 135 if(angle==180) {
12104404 6:0602a9e8118b 136 if(abs(xya.a-angle)<=5)
12104404 6:0602a9e8118b 137 s=SPEED_TURN_MIN;
12104404 6:0602a9e8118b 138 else
12104404 6:0602a9e8118b 139 s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
12104404 6:0602a9e8118b 140 motor1F=s;
12104404 6:0602a9e8118b 141 motor1B=s;
12104404 6:0602a9e8118b 142 motor2F=s;
12104404 6:0602a9e8118b 143 motor2B=s;
12104404 6:0602a9e8118b 144 if(xya.a>angle+5) {
12104404 6:0602a9e8118b 145 dir1=1;
12104404 6:0602a9e8118b 146 dir2=0;
12104404 6:0602a9e8118b 147 } else if(xya.a<angle-5) {
12104404 6:0602a9e8118b 148 dir1=0;
12104404 6:0602a9e8118b 149 dir2=1;
12104404 6:0602a9e8118b 150 } else {
12104404 6:0602a9e8118b 151 motor1F=0;
12104404 6:0602a9e8118b 152 motor1B=0;
12104404 6:0602a9e8118b 153 motor2F=0;
12104404 6:0602a9e8118b 154 motor2B=0;
12104404 6:0602a9e8118b 155 }
12104404 6:0602a9e8118b 156 } else if(angle==0) {
12104404 6:0602a9e8118b 157 if(xya.a>angle+5 && xya.a<180) {
12104404 6:0602a9e8118b 158 s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
12104404 6:0602a9e8118b 159 motor1F=s;
12104404 6:0602a9e8118b 160 motor1B=s;
12104404 6:0602a9e8118b 161 motor2F=s;
12104404 6:0602a9e8118b 162 motor2B=s;
12104404 6:0602a9e8118b 163 dir1=1;
12104404 6:0602a9e8118b 164 dir2=0;
12104404 6:0602a9e8118b 165 } else if(xya.a>180 && xya.a<355) {
12104404 6:0602a9e8118b 166 s=(SPEED_TURN_MAX*abs(xya.a-359)/180)+SPEED_TURN_MIN;
12104404 6:0602a9e8118b 167 motor1F=s;
12104404 6:0602a9e8118b 168 motor1B=s;
12104404 6:0602a9e8118b 169 motor2F=s;
12104404 6:0602a9e8118b 170 motor2B=s;
12104404 6:0602a9e8118b 171 dir1=0;
12104404 6:0602a9e8118b 172 dir2=1;
12104404 6:0602a9e8118b 173 } else {
12104404 6:0602a9e8118b 174 motor1F=0;
12104404 6:0602a9e8118b 175 motor1B=0;
12104404 6:0602a9e8118b 176 motor2F=0;
12104404 6:0602a9e8118b 177 motor2B=0;
12104404 6:0602a9e8118b 178 }
12104404 6:0602a9e8118b 179 }
12104404 6:0602a9e8118b 180 }