Code for our FYDP -only one IMU works right now -RTOS is working
Diff: bt_shell/shell/recon_fnt.h
- Revision:
- 0:964eb6a2ef00
diff -r 000000000000 -r 964eb6a2ef00 bt_shell/shell/recon_fnt.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bt_shell/shell/recon_fnt.h Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,92 @@ +void reset_position_fnt(int argc, char **argv) +{ + if(echo){ + bt.lock(); + bt.printf("recon position reset to x = 0, y = 0, angle = 0\r\n"); + bt.unlock(); + } + reckon.reset(); +} + +int Vav = 40; +float k = 3; + +void goto_coord_fnt(int argc, char **argv) +{ +//to fix for updated reckon +/* + if(argc != 3){ + if(echo){ + bt.lock(); + bt.printf("Usage: gotocoord [x] [y]\r\n"); + bt.unlock(); + return; + } + } + + float x_dest = atof( argv[1] ); + float y_dest = atof( argv[2] ); + + bt.lock(); + bt.printf("going to coordinates [%.2f, %.2f]...\r\n", x_dest, y_dest); + bt.printf("Press enter to exit\r\n"); + bt.unlock(); + + float return_angle_offset; + int Vt, Lspeed, Rspeed; + + char c = 0; + + while(c != '\r' && c != '\n' && reckon.dest_distance(x_dest, y_dest) > 20) + { + bt.lock(); + if(bt.readable()) + { + c = bt.getc(); + bt.unlock(); + }else + bt.unlock(); + + return_angle_offset = (reckon.dest_angle(x_dest, y_dest) - reckon.angle)*180/3.14159265359; + + if(return_angle_offset > 90){ + Vt =(int)(-k*(180-return_angle_offset)); + } + else if(return_angle_offset < -90) + Vt =(int)(k*(180+return_angle_offset)); + else + Vt =(int)(k*return_angle_offset); + + Lspeed = - Vt; + Rspeed = + Vt; + + if(abs(return_angle_offset)> 90) + { + Lspeed -= Vav; + Rspeed -= Vav; + } + else + { + Lspeed += Vav; + Rspeed += Vav; + } + + if(Lspeed > Vav*2) + Lspeed = Vav*2; + else if(Lspeed < -Vav*2) + Lspeed = -Vav*2; + + if(Rspeed > Vav*2) + Rspeed = Vav*2; + else if(Rspeed < -Vav*2) + Rspeed = -Vav*2; + + *motors.Left = Lspeed; + *motors.Right = Rspeed; + + Thread::wait(30); + } + *motors.Left = 0; + *motors.Right = 0; + */ +} \ No newline at end of file