Code for our FYDP -only one IMU works right now -RTOS is working
bt_shell/shell/recon_fnt.h
- Committer:
- majik
- Date:
- 2015-03-18
- Revision:
- 0:964eb6a2ef00
File content as of revision 0:964eb6a2ef00:
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; */ }