latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
Diff: main.cpp
- Revision:
- 4:6c8b844d291f
- Parent:
- 3:98ef5105926e
- Child:
- 5:d5c2e8f852fd
--- a/main.cpp Wed Apr 04 14:47:57 2018 +0000 +++ b/main.cpp Wed Apr 04 17:14:22 2018 +0000 @@ -46,8 +46,8 @@ float pos_down_start = 1400.00; float pos_up_start = 1000.00; -float down_degree = 80.00000000; -float up_degree = 15.00000000 ; +float down_degree = 0.00 ; +float up_degree = 0.00 ; float stepmin = 1; float round = 10; float waittime = 0.001 ; @@ -77,14 +77,16 @@ //pc.printf("malin"); //getvalue(); timer1.start(); // start timer counting - if (pc.getc() == '1') - { + //if (pc.getc() == '1') + //{ pc.printf("ma"); thread2.start(servo); - thread1.start(IMU); - } + //thread1.start(IMU); + //} } void cal_step_down(){ + pos_down_end_left = (1000.00 + ((700.00/90.00)*(down_degree))); + pos_down_end_right = (1070.00 + ((700.00/90.00)* (down_degree))) ; if (pos_down_end_right > pos_down_end_left){ step_down_right = (pos_down_end_right - pos_down_start)*stepmin/(pos_down_end_left - pos_down_start); step_down_left = stepmin; @@ -95,17 +97,19 @@ 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(){ + pos_up_end_left = 1000.00 + ((700.00/90.00)*(up_degree)); + pos_up_end_right = 1000.00 + ((700.00/90.00)* (up_degree)); if (pos_up_end_right > pos_up_end_left){ step_up_right = (pos_up_end_right - pos_up_start)*stepmin/(pos_up_end_left - pos_up_start); step_up_left = stepmin; @@ -116,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(){ @@ -247,9 +251,14 @@ void servo() { attitude_setup(); + getvalue(); pc.printf("start\n"); + //pc.printf("%f \n",down_degree ); + //pc.printf("%f \n",up_degree ); cal_step_down(); cal_step_up(); + //pc.printf("%f \n",down_degree ); + //pc.printf("%f \n",up_degree ); move(); } @@ -306,7 +315,7 @@ } break; } - pc.printf("%f \n",down_degree ); - pc.printf("%f \n",up_degree ); + //pc.printf("%f \n",down_degree ); + //pc.printf("%f \n",up_degree ); } \ No newline at end of file