Code for our FYDP -only one IMU works right now -RTOS is working
bt_shell/shell/recon_fnt.h@0:964eb6a2ef00, 2015-03-18 (annotated)
- Committer:
- majik
- Date:
- Wed Mar 18 22:23:48 2015 +0000
- Revision:
- 0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
majik | 0:964eb6a2ef00 | 1 | void reset_position_fnt(int argc, char **argv) |
majik | 0:964eb6a2ef00 | 2 | { |
majik | 0:964eb6a2ef00 | 3 | if(echo){ |
majik | 0:964eb6a2ef00 | 4 | bt.lock(); |
majik | 0:964eb6a2ef00 | 5 | bt.printf("recon position reset to x = 0, y = 0, angle = 0\r\n"); |
majik | 0:964eb6a2ef00 | 6 | bt.unlock(); |
majik | 0:964eb6a2ef00 | 7 | } |
majik | 0:964eb6a2ef00 | 8 | reckon.reset(); |
majik | 0:964eb6a2ef00 | 9 | } |
majik | 0:964eb6a2ef00 | 10 | |
majik | 0:964eb6a2ef00 | 11 | int Vav = 40; |
majik | 0:964eb6a2ef00 | 12 | float k = 3; |
majik | 0:964eb6a2ef00 | 13 | |
majik | 0:964eb6a2ef00 | 14 | void goto_coord_fnt(int argc, char **argv) |
majik | 0:964eb6a2ef00 | 15 | { |
majik | 0:964eb6a2ef00 | 16 | //to fix for updated reckon |
majik | 0:964eb6a2ef00 | 17 | /* |
majik | 0:964eb6a2ef00 | 18 | if(argc != 3){ |
majik | 0:964eb6a2ef00 | 19 | if(echo){ |
majik | 0:964eb6a2ef00 | 20 | bt.lock(); |
majik | 0:964eb6a2ef00 | 21 | bt.printf("Usage: gotocoord [x] [y]\r\n"); |
majik | 0:964eb6a2ef00 | 22 | bt.unlock(); |
majik | 0:964eb6a2ef00 | 23 | return; |
majik | 0:964eb6a2ef00 | 24 | } |
majik | 0:964eb6a2ef00 | 25 | } |
majik | 0:964eb6a2ef00 | 26 | |
majik | 0:964eb6a2ef00 | 27 | float x_dest = atof( argv[1] ); |
majik | 0:964eb6a2ef00 | 28 | float y_dest = atof( argv[2] ); |
majik | 0:964eb6a2ef00 | 29 | |
majik | 0:964eb6a2ef00 | 30 | bt.lock(); |
majik | 0:964eb6a2ef00 | 31 | bt.printf("going to coordinates [%.2f, %.2f]...\r\n", x_dest, y_dest); |
majik | 0:964eb6a2ef00 | 32 | bt.printf("Press enter to exit\r\n"); |
majik | 0:964eb6a2ef00 | 33 | bt.unlock(); |
majik | 0:964eb6a2ef00 | 34 | |
majik | 0:964eb6a2ef00 | 35 | float return_angle_offset; |
majik | 0:964eb6a2ef00 | 36 | int Vt, Lspeed, Rspeed; |
majik | 0:964eb6a2ef00 | 37 | |
majik | 0:964eb6a2ef00 | 38 | char c = 0; |
majik | 0:964eb6a2ef00 | 39 | |
majik | 0:964eb6a2ef00 | 40 | while(c != '\r' && c != '\n' && reckon.dest_distance(x_dest, y_dest) > 20) |
majik | 0:964eb6a2ef00 | 41 | { |
majik | 0:964eb6a2ef00 | 42 | bt.lock(); |
majik | 0:964eb6a2ef00 | 43 | if(bt.readable()) |
majik | 0:964eb6a2ef00 | 44 | { |
majik | 0:964eb6a2ef00 | 45 | c = bt.getc(); |
majik | 0:964eb6a2ef00 | 46 | bt.unlock(); |
majik | 0:964eb6a2ef00 | 47 | }else |
majik | 0:964eb6a2ef00 | 48 | bt.unlock(); |
majik | 0:964eb6a2ef00 | 49 | |
majik | 0:964eb6a2ef00 | 50 | return_angle_offset = (reckon.dest_angle(x_dest, y_dest) - reckon.angle)*180/3.14159265359; |
majik | 0:964eb6a2ef00 | 51 | |
majik | 0:964eb6a2ef00 | 52 | if(return_angle_offset > 90){ |
majik | 0:964eb6a2ef00 | 53 | Vt =(int)(-k*(180-return_angle_offset)); |
majik | 0:964eb6a2ef00 | 54 | } |
majik | 0:964eb6a2ef00 | 55 | else if(return_angle_offset < -90) |
majik | 0:964eb6a2ef00 | 56 | Vt =(int)(k*(180+return_angle_offset)); |
majik | 0:964eb6a2ef00 | 57 | else |
majik | 0:964eb6a2ef00 | 58 | Vt =(int)(k*return_angle_offset); |
majik | 0:964eb6a2ef00 | 59 | |
majik | 0:964eb6a2ef00 | 60 | Lspeed = - Vt; |
majik | 0:964eb6a2ef00 | 61 | Rspeed = + Vt; |
majik | 0:964eb6a2ef00 | 62 | |
majik | 0:964eb6a2ef00 | 63 | if(abs(return_angle_offset)> 90) |
majik | 0:964eb6a2ef00 | 64 | { |
majik | 0:964eb6a2ef00 | 65 | Lspeed -= Vav; |
majik | 0:964eb6a2ef00 | 66 | Rspeed -= Vav; |
majik | 0:964eb6a2ef00 | 67 | } |
majik | 0:964eb6a2ef00 | 68 | else |
majik | 0:964eb6a2ef00 | 69 | { |
majik | 0:964eb6a2ef00 | 70 | Lspeed += Vav; |
majik | 0:964eb6a2ef00 | 71 | Rspeed += Vav; |
majik | 0:964eb6a2ef00 | 72 | } |
majik | 0:964eb6a2ef00 | 73 | |
majik | 0:964eb6a2ef00 | 74 | if(Lspeed > Vav*2) |
majik | 0:964eb6a2ef00 | 75 | Lspeed = Vav*2; |
majik | 0:964eb6a2ef00 | 76 | else if(Lspeed < -Vav*2) |
majik | 0:964eb6a2ef00 | 77 | Lspeed = -Vav*2; |
majik | 0:964eb6a2ef00 | 78 | |
majik | 0:964eb6a2ef00 | 79 | if(Rspeed > Vav*2) |
majik | 0:964eb6a2ef00 | 80 | Rspeed = Vav*2; |
majik | 0:964eb6a2ef00 | 81 | else if(Rspeed < -Vav*2) |
majik | 0:964eb6a2ef00 | 82 | Rspeed = -Vav*2; |
majik | 0:964eb6a2ef00 | 83 | |
majik | 0:964eb6a2ef00 | 84 | *motors.Left = Lspeed; |
majik | 0:964eb6a2ef00 | 85 | *motors.Right = Rspeed; |
majik | 0:964eb6a2ef00 | 86 | |
majik | 0:964eb6a2ef00 | 87 | Thread::wait(30); |
majik | 0:964eb6a2ef00 | 88 | } |
majik | 0:964eb6a2ef00 | 89 | *motors.Left = 0; |
majik | 0:964eb6a2ef00 | 90 | *motors.Right = 0; |
majik | 0:964eb6a2ef00 | 91 | */ |
majik | 0:964eb6a2ef00 | 92 | } |