![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
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?
User | Revision | Line number | New 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 | } |