Sopitchaya Lummaetee
/
TurtleBot_V555
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Servo mbed-rtos mbed
Fork of
TurtleBot_V55
by 
Sopitchaya Lummaetee
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "Servo.h" 00003 #include "rtos.h" 00004 #include "attitude.h" 00005 00006 Serial pc(USBTX, USBRX); 00007 //Serial bt(A7,A2); 00008 Timer timer1; 00009 Timer timerwalk; 00010 Thread thread1; 00011 Thread thread2; 00012 00013 void IMU() 00014 { 00015 while(1) { 00016 if (timer1.read_us() >=1000)// read time in ms 00017 { 00018 attitude_get(); 00019 //pc.printf("IMU \n"); 00020 pc.printf(" %f \t", ax*10 ); 00021 pc.printf(" %f \t", ay*10 ); 00022 pc.printf(" %f \t", az*10 -10); //cm/s*s 00023 00024 pc.printf(" %f \t", gx ); 00025 pc.printf(" %f \t", gy ); 00026 pc.printf(" %f \t", gz ); //deg/s */ 00027 pc.printf("%.0f\t %.0f \t %.0f \n\r", roll, pitch, yaw); 00028 00029 timer1.reset(); // reset timer 00030 } 00031 } 00032 } 00033 00034 Servo Servo1(D6); 00035 Servo Servo2(D8); 00036 Servo Servo3(D9); 00037 Servo Servo4(D10); 00038 00039 int value; 00040 int walking_time; 00041 00042 void servo_Right(); 00043 void move(); 00044 void cal_step_down(); 00045 void cal_step_up(); 00046 void servo(); 00047 void getvalue(); 00048 00049 float pos_down_start = 1400.00; 00050 float pos_up_start = 1000.00; 00051 float down_degree = 0.00 ; 00052 float up_degree = 0.00 ; 00053 float stepmin = 1; 00054 float round = 5; 00055 float waittime = 0.001 ; 00056 00057 float pos_down_left = 1400.00; 00058 float pos_up_left = 1000.00; 00059 float pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); //left down //90 ใน80 นอก(45)+7 ใน85 นอก+5 00060 float pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); //left up// 15 , 30+10 , 45-1.75 , 60-5 , 75-5 , 45+5 00061 float state_count_left = 1; 00062 float round_count_left = 1; 00063 float step_down_left; 00064 float step_up_left; 00065 00066 float pos_down_right = 1400.00; 00067 float pos_up_right = 1000.00; 00068 float pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; //right down //99 00069 float pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); //right up// 15 , 30-10 , 45+1.75 , 60+5 , 75+5 , 45-5 00070 float state_count_right = 1; 00071 float round_count_right = 1; 00072 float step_up_right; 00073 float step_down_right; 00074 00075 00076 00077 int main() { 00078 pc.baud(1000000); 00079 //pc.printf("malin"); 00080 timer1.start(); // start timer counting 00081 //if (pc.getc() == '1') 00082 //{ 00083 thread2.start(servo); 00084 //thread1.start(IMU); 00085 //} 00086 } 00087 void cal_step_down(){ 00088 pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); 00089 pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; 00090 if (pos_down_end_right > pos_down_end_left){ 00091 step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); 00092 step_down_left = stepmin; 00093 } else if (pos_down_end_right < pos_down_end_left){ 00094 step_down_right = stepmin; 00095 step_down_left = (pos_down_end_left - pos_down_start)*stepmin/(pos_down_end_right - pos_down_start); 00096 } else{ 00097 step_down_right = stepmin; 00098 step_down_left = stepmin; 00099 } 00100 /*pc.printf("pos_down_right"); 00101 pc.printf("%f\n",pos_down_end_right); 00102 pc.printf("pos_down_left"); 00103 pc.printf("%f\n",pos_down_end_left); 00104 pc.printf("step_down_right"); 00105 pc.printf("%f\n",step_down_right); 00106 pc.printf("step_down_left"); 00107 pc.printf("%f\n",step_down_left); */ 00108 } 00109 00110 void cal_step_up(){ 00111 pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); 00112 pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); 00113 if (pos_up_end_right > pos_up_end_left){ 00114 step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); 00115 step_up_left = stepmin; 00116 } else if (pos_up_end_right < pos_up_end_left){ 00117 step_up_right = stepmin; 00118 step_up_left = (pos_up_end_left - pos_up_start)*stepmin/(pos_up_end_right - pos_up_start); 00119 } else{ 00120 step_up_right = stepmin; 00121 step_up_left = stepmin; 00122 } 00123 /*pc.printf("pos_up_right"); 00124 pc.printf("%f\n",pos_up_end_right); 00125 pc.printf("pos_up_left"); 00126 pc.printf("%f\n",pos_up_end_left); 00127 pc.printf("step_up_right"); 00128 pc.printf("%f\n",step_up_right);; 00129 pc.printf("step_up_left"); 00130 pc.printf("%f\n",step_up_left); */ 00131 } 00132 00133 void move(){ 00134 Servo1.Enable(1000,2000); 00135 Servo2.Enable(1000,2000); 00136 Servo3.Enable(1000,2000); 00137 Servo4.Enable(1000,2000); 00138 while(1) { 00139 servo_Right(); 00140 if(state_count_left == 1) { 00141 Servo1.SetPosition(pos_down_left); 00142 wait(waittime); 00143 pos_down_left = pos_down_left + step_down_left; 00144 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left == pos_up_start) { 00145 state_count_left = 2; 00146 } 00147 /*pc.printf("LAD"); 00148 pc.printf("%f\n",pos_down_left); 00149 pc.printf("LAP"); 00150 pc.printf("%f\n",pos_up_left);*/ 00151 } else if(state_count_left == 2) { 00152 Servo2.SetPosition(pos_up_left); 00153 wait(waittime); 00154 pos_up_left = pos_up_left + step_up_left; 00155 if(pos_down_left >= pos_down_end_left + step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { 00156 state_count_left = 3; 00157 } 00158 /*pc.printf("LBD"); 00159 pc.printf("%f\n",pos_down_left); 00160 pc.printf("LBP"); 00161 pc.printf("%f\n",pos_up_left);*/ 00162 } else if(state_count_left == 3) { 00163 Servo1.SetPosition(pos_down_left); 00164 wait(waittime); 00165 pos_down_left = pos_down_left - step_down_left; 00166 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left >= pos_up_end_left + step_up_left) { 00167 state_count_left = 4; 00168 } 00169 /*pc.printf("LCD"); 00170 pc.printf("%f\n",pos_down_left); 00171 pc.printf("LCP"); 00172 pc.printf("%f\n",pos_up_left);*/ 00173 } else if(state_count_left == 4) { 00174 Servo2.SetPosition(pos_up_left); 00175 wait(waittime); 00176 pos_up_left = pos_up_left - step_up_left; 00177 if(pos_down_left <= pos_down_start - step_down_left and pos_up_left <= pos_up_start - step_up_left) { 00178 state_count_left = 0; 00179 } 00180 /*pc.printf("LDD"); 00181 pc.printf("%f\n",pos_down_left); 00182 pc.printf("LDP"); 00183 pc.printf("%f\n",pos_up_left);*/ 00184 } else if (state_count_left == 0 and round_count_left < round) { 00185 round_count_left = round_count_left+1; 00186 state_count_left = 1; 00187 pos_down_left = pos_down_start; 00188 pos_up_left = pos_up_start; 00189 } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){ 00190 pc.printf("Finish \n"); 00191 walking_time = timerwalk.read_ms(); 00192 thread1.terminate(); 00193 pc.printf("Walking time = %d \n", walking_time); 00194 break; 00195 } 00196 } 00197 } 00198 00199 void servo_Right() 00200 { 00201 if(state_count_right == 1) { 00202 Servo3.SetPosition(pos_down_right); 00203 wait(waittime); 00204 pos_down_right = pos_down_right + step_down_right; 00205 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right == pos_up_start) { 00206 state_count_right = 2; 00207 } 00208 /*pc.printf("RAD"); 00209 pc.printf("%f\n",pos_down_right); 00210 pc.printf("RAP"); 00211 pc.printf("%f\n",pos_up_right);*/ 00212 } else if(state_count_right == 2) { 00213 Servo4.SetPosition(pos_up_right); 00214 wait(waittime); 00215 pos_up_right = pos_up_right + step_up_right; 00216 if(pos_down_right >= pos_down_end_right + step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { 00217 state_count_right = 3; 00218 } 00219 /*pc.printf("RBD"); 00220 pc.printf("%f\n",pos_down_right); 00221 pc.printf("RBP"); 00222 pc.printf("%f\n",pos_up_right);*/ 00223 } else if(state_count_right == 3) { 00224 Servo3.SetPosition(pos_down_right); 00225 wait(waittime); 00226 pos_down_right = pos_down_right - step_down_right; 00227 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right >= pos_up_end_right + step_up_right) { 00228 state_count_right = 4; 00229 } 00230 /*pc.printf("RCD"); 00231 pc.printf("%f\n",pos_down_right); 00232 pc.printf("RCP"); 00233 pc.printf("%f\n",pos_up_right);*/ 00234 } else if(state_count_right == 4) { 00235 Servo4.SetPosition(pos_up_right); 00236 wait(waittime); 00237 pos_up_right = pos_up_right - step_up_right; 00238 if(pos_down_right <= pos_down_start - step_down_right and pos_up_right <= pos_up_start - step_up_right) { 00239 state_count_right = 0; 00240 } 00241 /*pc.printf("RDD"); 00242 pc.printf("%f\n",pos_down_right); 00243 pc.printf("RDP"); 00244 pc.printf("%f\n",pos_up_right);*/ 00245 } else if (state_count_right == 0 and round_count_right < round) { 00246 round_count_right = round_count_right+1; 00247 state_count_right = 1; 00248 pos_down_right = pos_down_start; 00249 pos_up_right = pos_up_start; 00250 } 00251 } 00252 00253 00254 void servo() { 00255 attitude_setup(); 00256 getvalue(); 00257 pc.printf("start\n"); 00258 thread1.start(IMU); 00259 timerwalk.start(); // start timer counting 00260 //pc.printf("%f \n",down_degree ); 00261 //pc.printf("%f \n",up_degree ); 00262 cal_step_down(); 00263 cal_step_up(); 00264 //pc.printf("%f \n",down_degree ); 00265 //pc.printf("%f \n",up_degree ); 00266 move(); 00267 } 00268 00269 void getvalue() { 00270 pc.printf("case 1 = 90-15 \n"); 00271 pc.printf("case 2 = 90-30 \n"); 00272 pc.printf("case 3 = 90-45 \n"); 00273 pc.printf("case 4 = 90-60 \n"); 00274 pc.printf("case 5 = 90-75 \n"); 00275 pc.printf("case 6 = 80-45 \n"); 00276 pc.printf("case 7 = 85-45 \n"); 00277 pc.printf("case 8 = 95-45 \n"); 00278 pc.printf("case 9 = 100-45 \n"); 00279 value = pc.getc() ; 00280 switch (value) { 00281 case '1': { 00282 down_degree = 90.00 ; 00283 up_degree = 15.00 ; 00284 break; 00285 } 00286 case '2': { 00287 down_degree = 90.00 ; 00288 up_degree = 30.00 ; 00289 break; 00290 } 00291 case '3': { 00292 down_degree = 90.00 ; 00293 up_degree = 45.00 ; 00294 break; 00295 } 00296 case '4': { 00297 down_degree = 90.00 ; 00298 up_degree = 60.00 ; 00299 break; 00300 } 00301 case '5': { 00302 down_degree = 90.00 ; 00303 up_degree = 75.00 ; 00304 break; 00305 } 00306 case '6': { 00307 down_degree = 80.00 ; 00308 up_degree = 45.00 ; 00309 break; 00310 } 00311 case '7': { 00312 down_degree = 85.00 ; 00313 up_degree = 45.00 ; 00314 break; 00315 } 00316 case '8': { 00317 down_degree = 95.00 ; 00318 up_degree = 45.00 ; 00319 break; 00320 } 00321 case '9': { 00322 down_degree = 100.00 ; 00323 up_degree = 45.00 ; 00324 break; 00325 } 00326 break; 00327 } 00328 //pc.printf("%f \n",down_degree ); 00329 //pc.printf("%f \n",up_degree ); 00330 00331 }
Generated on Sat Aug 13 2022 20:21:47 by
1.7.2