Diff: RobotServo.cpp

Revision:

4:b0967990e390

Parent:

3:197b748a397a

--- a/RobotServo.cpp	Wed Jun 22 04:35:18 2016 +0000
+++ b/RobotServo.cpp	Wed Jul 06 06:54:15 2016 +0000
@@ -157,23 +157,24 @@
 }
 
 void pwm_left(float angle_up, float angle_mid, float angle_down){   //Left arm
-  pwm_ls.pulsewidth((angle_up+10.0f) / 90.0f * 0.0008f + 0.002297f);
-  pwm_le.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0007044f);
-  pwm_lw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
+  pwm_ls.pulsewidth((angle_up+10) / 90.0f * 0.0008f + 0.0023f);
+  pwm_le.pulsewidth(angle_mid * 0.000009f + 0.0007044f);
+  pwm_lw.pulsewidth(-angle_down * 0.000009f + 0.001505f);
 //  OCR3B = 574.0+(400.0/180.0)*(angle_up+10); //pin 2
 //  OCR2B = 44.0+(100.0/180.0)*angle_mid; //pin 9
 //  OCR0B = 94+(100.0/180.0)*(-1)*angle_down; // pin 4
 }
 void pwm_right(float angle_up, float angle_mid, float angle_down){  //Right arm
-  pwm_rs.pulsewidth((angle_up-3.0f) / 90.0f * 0.0008f + 0.0007f);
-  pwm_re.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0023f);
-  pwm_rw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
+  pwm_rs.pulsewidth((angle_up-3) / 90.0f * 0.0008f + 0.0007f);
+  pwm_re.pulsewidth(angle_mid * 0.000009f + 0.0023f);
+  pwm_rw.pulsewidth(-angle_down * 0.000009f + 0.001505f);
 //  OCR1B = 175+(400.0/180.0)*(angle_up-3); //pin 12
 //  OCR2A = 144.0+(100.0/180.0)*angle_mid; //pin 10
 //  OCR0A = 94+(100.0/180.0)*(-1)*angle_down; // pin 13
 }
 void lookuptable_steering(int8_t sensor_output){
-    int i = sensor_output/2;
+//    int i = sensor_output/2;
+    int i = sensor_output >> 1;
     int q1 = servoangle_left[20+i][0];
     int q2 = servoangle_left[21+i][0];
     int q3 = servoangle_left[20+i][1];
@@ -190,7 +191,7 @@
     float x_L2 = (q4-q3)*(sensor_output-2*i)/2 + q3;
     float x_L3 = (q6-q5)*(sensor_output-2*i)/2 + q5;
     float x_R1 = (q8-q7)*(sensor_output-2*i)/2 + q7;
-    float x_R2 = (q10-q9)*(sensor_output-2*i)/2 + q9;
+    float x_R2 = (q10-q9)*((sensor_output-(i << 1)) >> 1) + q9;
     float x_R3 = (q12-q11)*(sensor_output-2*i)/2 + q11;
     pwm_left(x_L1,x_L2,x_L3);
     pwm_right(x_R1,x_R2,x_R3);
@@ -198,19 +199,19 @@
 
 void pwm_left_foot(float angle_1,float angle_2,float angle_3,float angle_4){
   pwm_lb.pulsewidth(POWER_REDU_PW);
-  pwm_ll.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.0007f);
-  pwm_lk.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.002297f);
-  pwm_la.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
+  pwm_ll.pulsewidth(angle_2 * 0.000008f + 0.0007f);
+  pwm_lk.pulsewidth(angle_3 * 0.000008f + 0.0023f);
+  pwm_la.pulsewidth(-angle_4 * 0.000008f + 0.001513f);
 //  OCR4A = 25;//(-(angle_1 + 4.0) / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6
 //  OCR3A = 175 + (400/180)*angle_2; // Pin 5
 //  OCR3C = 574 + (400/180)*angle_3; // Pin 3
 //  OCR5B = 378 + (400/180)*angle_4*(-1); // Pin 45
 }
 void pwm_right_foot(float angle_1,float angle_2,float angle_3,float angle_4){
-  pwm_rb.pulsewidth(8.5f / 90.0f * 0.0008f + 0.0015f);
-  pwm_rl.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.002297f);
-  pwm_rk.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.0007f);
-  pwm_ra.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
+  pwm_rb.pulsewidth(7.5f / 90.0f * 0.0008f + 0.0015f);
+  pwm_rl.pulsewidth(angle_2 * 0.000008f + 0.0023f);
+  pwm_rk.pulsewidth(angle_3 * 0.000008f + 0.0007f);
+  pwm_ra.pulsewidth(-angle_4 * 0.000008f + 0.001513f);
 //  OCR4B = (8.5 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 7
 //  OCR4C = 574 + (400/180)*angle_2; // Pin 8
 //  OCR1A = 175 + (400/180)*angle_3; // Pin 11
@@ -230,22 +231,24 @@
 }
 void reset_left_butt(void)
 {
-  pwm_lb.pulsewidth(-12.0f / 90.0f * 0.0008f + 0.0015f);
+  pwm_lb.pulsewidth(-11.0f / 90.0f * 0.0008f + 0.0015f);
 //  OCR4A = (-12.0 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6, left butt
 }
 void reset_pos(void)
 {
   pwm_left_foot(8.86  ,115  ,-80  ,-12.739);
   reset_left_butt();
-  pwm_right_foot(-6.11 ,-48.68 ,32.24  ,12.15);
-//  pwm_right_foot(-6.11 ,-50.68 ,38.24  ,4.15);
+//  pwm_right_foot(-6.11 ,-46.68 ,30.24  ,14.15);
+  pwm_right_foot(-6.11 ,-44.68 ,27.24  ,16.15);
+//  pwm_right_foot(-6.11 ,-48.68 ,32.24  ,12.15);
 }
 void stop_pos(void)
 {
   pwm_left_foot(8.86  ,115  ,-80  ,-12.739);
   reset_left_butt();
+  pwm_right_foot(-6.11 ,-46.68 ,30.24  ,14.15);
 //  pwm_right_foot(-6.11 ,-48.68 ,32.24  ,12.15);
-  pwm_right_foot(-6.11 ,-50.68 ,38.24  ,4.15);
+//  pwm_right_foot(-6.11 ,-49.68 ,35.24  ,8.15);
 }
 int angle_to_duty(int resolution, float degree)
 {//+-90 degrees