![](/media/cache/group/GF5017-1-1.jpg.50x50_q85.jpg)
latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V555 by
Revision 5:d5c2e8f852fd, committed 2018-04-06
- Comitter:
- 59010050
- Date:
- Fri Apr 06 08:33:48 2018 +0000
- Parent:
- 4:6c8b844d291f
- Commit message:
- latest
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 6c8b844d291f -r d5c2e8f852fd main.cpp --- a/main.cpp Wed Apr 04 17:14:22 2018 +0000 +++ b/main.cpp Fri Apr 06 08:33:48 2018 +0000 @@ -6,6 +6,7 @@ Serial pc(USBTX, USBRX); //Serial bt(A7,A2); Timer timer1; +Timer timerwalk; Thread thread1; Thread thread2; @@ -36,6 +37,7 @@ Servo Servo4(D10); int value; +int walking_time; void servo_Right(); void move(); @@ -49,7 +51,7 @@ float down_degree = 0.00 ; float up_degree = 0.00 ; float stepmin = 1; -float round = 10; +float round = 5; float waittime = 0.001 ; float pos_down_left = 1400.00; @@ -75,14 +77,12 @@ int main() { pc.baud(1000000); //pc.printf("malin"); - //getvalue(); timer1.start(); // start timer counting //if (pc.getc() == '1') - //{ - pc.printf("ma"); + //{ thread2.start(servo); //thread1.start(IMU); - //} + //} } void cal_step_down(){ pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); @@ -97,14 +97,14 @@ step_down_right = stepmin; step_down_left = stepmin; } - pc.printf("pos_down_right"); + /*pc.printf("pos_down_right"); pc.printf("%f\n",pos_down_end_right); pc.printf("pos_down_left"); pc.printf("%f\n",pos_down_end_left); pc.printf("step_down_right"); pc.printf("%f\n",step_down_right); pc.printf("step_down_left"); - pc.printf("%f\n",step_down_left); + pc.printf("%f\n",step_down_left); */ } void cal_step_up(){ @@ -120,14 +120,14 @@ step_up_right = stepmin; step_up_left = stepmin; } - pc.printf("pos_up_right"); + /*pc.printf("pos_up_right"); pc.printf("%f\n",pos_up_end_right); pc.printf("pos_up_left"); pc.printf("%f\n",pos_up_end_left); pc.printf("step_up_right"); pc.printf("%f\n",step_up_right);; pc.printf("step_up_left"); - pc.printf("%f\n",step_up_left); + pc.printf("%f\n",step_up_left); */ } void move(){ @@ -187,8 +187,10 @@ pos_down_left = pos_down_start; pos_up_left = pos_up_start; } else if (state_count_left == 0 and round_count_left == round and state_count_right == 0 and round_count_right == round){ - pc.printf("Finish"); + pc.printf("Finish \n"); + walking_time = timerwalk.read_ms(); thread1.terminate(); + pc.printf("Walking time = %d \n", walking_time); break; } } @@ -253,6 +255,8 @@ attitude_setup(); getvalue(); pc.printf("start\n"); + thread1.start(IMU); + timerwalk.start(); // start timer counting //pc.printf("%f \n",down_degree ); //pc.printf("%f \n",up_degree ); cal_step_down(); @@ -267,10 +271,11 @@ pc.printf("case 2 = 90-30 \n"); pc.printf("case 3 = 90-45 \n"); pc.printf("case 4 = 90-60 \n"); - pc.printf("case 5 = 80-45 \n"); - pc.printf("case 6 = 85-45 \n"); - pc.printf("case 7 = 95-45 \n"); - pc.printf("case 8 = 100-45 \n"); + pc.printf("case 5 = 90-75 \n"); + pc.printf("case 6 = 80-45 \n"); + pc.printf("case 7 = 85-45 \n"); + pc.printf("case 8 = 95-45 \n"); + pc.printf("case 9 = 100-45 \n"); value = pc.getc() ; switch (value) { case '1': { @@ -294,21 +299,26 @@ break; } case '5': { + down_degree = 90.00 ; + up_degree = 75.00 ; + break; + } + case '6': { down_degree = 80.00 ; up_degree = 45.00 ; break; } - case '6': { + case '7': { down_degree = 85.00 ; up_degree = 45.00 ; break; } - case '7': { + case '8': { down_degree = 95.00 ; up_degree = 45.00 ; break; } - case '8': { + case '9': { down_degree = 100.00 ; up_degree = 45.00 ; break;