Code for our FYDP -only one IMU works right now -RTOS is working
Embed:
(wiki syntax)
Show/hide line numbers
recon_fnt.h
00001 void reset_position_fnt(int argc, char **argv) 00002 { 00003 if(echo){ 00004 bt.lock(); 00005 bt.printf("recon position reset to x = 0, y = 0, angle = 0\r\n"); 00006 bt.unlock(); 00007 } 00008 reckon.reset(); 00009 } 00010 00011 int Vav = 40; 00012 float k = 3; 00013 00014 void goto_coord_fnt(int argc, char **argv) 00015 { 00016 //to fix for updated reckon 00017 /* 00018 if(argc != 3){ 00019 if(echo){ 00020 bt.lock(); 00021 bt.printf("Usage: gotocoord [x] [y]\r\n"); 00022 bt.unlock(); 00023 return; 00024 } 00025 } 00026 00027 float x_dest = atof( argv[1] ); 00028 float y_dest = atof( argv[2] ); 00029 00030 bt.lock(); 00031 bt.printf("going to coordinates [%.2f, %.2f]...\r\n", x_dest, y_dest); 00032 bt.printf("Press enter to exit\r\n"); 00033 bt.unlock(); 00034 00035 float return_angle_offset; 00036 int Vt, Lspeed, Rspeed; 00037 00038 char c = 0; 00039 00040 while(c != '\r' && c != '\n' && reckon.dest_distance(x_dest, y_dest) > 20) 00041 { 00042 bt.lock(); 00043 if(bt.readable()) 00044 { 00045 c = bt.getc(); 00046 bt.unlock(); 00047 }else 00048 bt.unlock(); 00049 00050 return_angle_offset = (reckon.dest_angle(x_dest, y_dest) - reckon.angle)*180/3.14159265359; 00051 00052 if(return_angle_offset > 90){ 00053 Vt =(int)(-k*(180-return_angle_offset)); 00054 } 00055 else if(return_angle_offset < -90) 00056 Vt =(int)(k*(180+return_angle_offset)); 00057 else 00058 Vt =(int)(k*return_angle_offset); 00059 00060 Lspeed = - Vt; 00061 Rspeed = + Vt; 00062 00063 if(abs(return_angle_offset)> 90) 00064 { 00065 Lspeed -= Vav; 00066 Rspeed -= Vav; 00067 } 00068 else 00069 { 00070 Lspeed += Vav; 00071 Rspeed += Vav; 00072 } 00073 00074 if(Lspeed > Vav*2) 00075 Lspeed = Vav*2; 00076 else if(Lspeed < -Vav*2) 00077 Lspeed = -Vav*2; 00078 00079 if(Rspeed > Vav*2) 00080 Rspeed = Vav*2; 00081 else if(Rspeed < -Vav*2) 00082 Rspeed = -Vav*2; 00083 00084 *motors.Left = Lspeed; 00085 *motors.Right = Rspeed; 00086 00087 Thread::wait(30); 00088 } 00089 *motors.Left = 0; 00090 *motors.Right = 0; 00091 */ 00092 }
Generated on Wed Jul 13 2022 10:32:23 by 1.7.2