Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }