Chris LU / Mbed 2 deprecated Nucleo_ZTC_20160607_oldest

code of robot bike

Dependencies:   SDFileSystem mbed

Fork of Robot_Bicycle by Chris LU

RobotServo.cpp@1:709be64ca63c, 2016-06-21 (annotated)

Committer:
YCTung
Date:
Tue Jun 21 18:11:46 2016 +0000
Revision:
1:709be64ca63c
Parent:
0:830ddddc129f
Child:
3:197b748a397a
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:830ddddc129f 1 #include "mbed.h"
YCTung 0:830ddddc129f 2 #include "RobotServo.h"
YCTung 0:830ddddc129f 3
YCTung 0:830ddddc129f 4 PwmOut pwm_lw(D2); //left wrist
YCTung 0:830ddddc129f 5 PwmOut pwm_le(D3); //left elbow
YCTung 0:830ddddc129f 6 PwmOut pwm_ls(D4); //left shoulder
YCTung 0:830ddddc129f 7 PwmOut pwm_la(D5); //left ankle
YCTung 0:830ddddc129f 8 PwmOut pwm_lk(D6); //left knee
YCTung 0:830ddddc129f 9 PwmOut pwm_ll(D7); //left leg
YCTung 0:830ddddc129f 10 PwmOut pwm_lb(D8); //left butt
YCTung 0:830ddddc129f 11 PwmOut pwm_rb(D10); //right butt
YCTung 0:830ddddc129f 12 PwmOut pwm_rl(PA_11);//right leg
YCTung 0:830ddddc129f 13 PwmOut pwm_rk(D13); //right knee
YCTung 0:830ddddc129f 14 PwmOut pwm_ra(D14); //right ankle
YCTung 0:830ddddc129f 15 PwmOut pwm_rs(D15); //right shoulder
YCTung 1:709be64ca63c 16 PwmOut pwm_re(PC_9); //right elbow
YCTung 1:709be64ca63c 17 PwmOut pwm_rw(PC_8); //right wrist
YCTung 0:830ddddc129f 18
YCTung 0:830ddddc129f 19 const int servoangle_left[TOT_ARM_POS][3] = { {-7,0,-25},{-5,0,-30},{-6,0,-28},{-7,0,-26},{-27,28,-33},{-36,39,-32},{-44,46,-30},{-50,52,-28},{-56,57,-26},{-61,62,-24},
YCTung 0:830ddddc129f 20 {-66,65,-21},{-70,68,-19},{-75,71,-17},{-79,74,-14},{-83,76,-12},{-87,78,-10},{-90,80,-7},{-94,81,-5},{-97,83,-2},{-101,84,0},
YCTung 0:830ddddc129f 21 {-104,85,3},{-107,86,5},{-110,87,7},{-113,87,10},{-115,88,12},{-118,88,14},{-121,89,16},{-123,89,18},{-125,89,20},{-127,89,22},
YCTung 0:830ddddc129f 22 {-130,89,23},{-131,89,25},{-133,88,26},{-135,88,27},{-136,88,28},{-137,87,29},{-138,87,30},{-139,86,30},{-140,86,30},{-140,85,29},{-140,84,28}};
YCTung 0:830ddddc129f 23 const int servoangle_right[TOT_ARM_POS][3] = {{140,-84,-28},{140,-85,-29},{140,-86,-30},{139,-86,-30},{138,-87,-30},{137,-87,-29},{136,-88,-28},{135,-88,-27},{133,-88,-26},
YCTung 0:830ddddc129f 24 {131,-89,-25},{130,-89,-23},{127,-89,-22},{125,-89,-20},{123,-89,-18},{121,-89,-16},{118,-88,-14},{115,-88,-12},{113,-87,-10},{110,-87,-7},
YCTung 0:830ddddc129f 25 {107,-86,-5},{104,-85,-3},{101,-84,0},{97,-83,2},{94,-81,5},{90,-80,7},{87,-78,10},{83,-76,12},{79,-74,14},{75,-71,17},{70,-68,19},
YCTung 0:830ddddc129f 26 {66,-65,21},{61,-62,24},{56,-57,26},{50,-52,28},{44,-46,30},{36,-39,32},{27,-28,33},{7,0,26},{6,0,28},{5,0,30},{7,0,25}};
YCTung 0:830ddddc129f 27
YCTung 0:830ddddc129f 28 const float footangle_left[TOT_FOOT_POS][4]= {
YCTung 0:830ddddc129f 29 {9.24 ,115.5 ,-98 ,-6}, //0
YCTung 0:830ddddc129f 30 {9.24 ,115.5 ,-95.5 ,-9}, //1
YCTung 0:830ddddc129f 31 {9.24 ,116 ,-93 ,-11}, //2
YCTung 0:830ddddc129f 32 {9.25 ,116 ,-90.2 ,-11}, //3
YCTung 0:830ddddc129f 33 {9.27 ,115 ,-84.98 ,-14.13}, //4
YCTung 0:830ddddc129f 34 {9.065 ,115 ,-83.38 ,-13.5}, //5
YCTung 0:830ddddc129f 35 {8.86 ,115 ,-80 ,-12.739}, //6
YCTung 0:830ddddc129f 36 {8.86 ,115 ,-80 ,-12.739}, //s1 for stop mode (from 6)
YCTung 0:830ddddc129f 37 {8.54 ,113 ,-78.89 ,-11.36}, //s2 for stop mode (from 7)
YCTung 0:830ddddc129f 38 {8.22 ,113 ,-78 ,-6}, //s3 for stop mode (from 8)
YCTung 0:830ddddc129f 39 {8.22 ,106 ,-73 ,-5}, //s4 for stop mode (from 9)
YCTung 0:830ddddc129f 40 {8.22 ,101 ,-69 ,-2}, //s5 for stop mode (from 10)
YCTung 0:830ddddc129f 41 {7.99 ,100 ,-64 ,0.2}, //s6 for stop mode (from 11)
YCTung 0:830ddddc129f 42 {7.99 ,96 ,-61 ,3}, //s7 for stop mode (from 12)
YCTung 0:830ddddc129f 43 {8.54 ,113 ,-78.89 ,-11.36}, //7
YCTung 0:830ddddc129f 44 {8.22 ,113 ,-78 ,-6}, //8
YCTung 0:830ddddc129f 45 {8.22 ,106 ,-73 ,-5}, //9
YCTung 0:830ddddc129f 46 {8.22 ,101 ,-69 ,-2}, //10
YCTung 0:830ddddc129f 47 {7.99 ,100 ,-64 ,0.2}, //11
YCTung 0:830ddddc129f 48 {7.99 ,96 ,-61 ,3}, //12
YCTung 0:830ddddc129f 49 {7.99 ,92 ,-59.5 ,6}, //13
YCTung 0:830ddddc129f 50 {7.86 ,89 ,-57.04 ,8.5}, //14
YCTung 0:830ddddc129f 51 {7.73 ,87 ,-56 ,10}, //15
YCTung 0:830ddddc129f 52 {7.63 ,84 ,-56 ,12}, //16
YCTung 0:830ddddc129f 53 {7.53 ,82 ,-55 ,14}, //17
YCTung 0:830ddddc129f 54 {5.77 ,80 ,-53 ,15}, //18
YCTung 0:830ddddc129f 55 {4.01 ,74 ,-53 ,16}, //19
YCTung 0:830ddddc129f 56 {4.485 ,70 ,-52 ,18.5}, //20
YCTung 0:830ddddc129f 57 {4.96 ,64 ,-45.89 ,14.6}, //21
YCTung 0:830ddddc129f 58 {5.44 ,62 ,-43.57 ,14}, //22
YCTung 0:830ddddc129f 59 {5.92 ,58 ,-44.25 ,14.52}, //23
YCTung 0:830ddddc129f 60 {5.92 ,56 ,-43.485,15.5}, //24
YCTung 0:830ddddc129f 61 {5.91 ,54 ,-44.72 ,16.5}, //25
YCTung 0:830ddddc129f 62 {5.91 ,54.45 ,-45 ,16}, //26
YCTung 0:830ddddc129f 63 {5.91 ,56.4 ,-51.5 ,13.53}, //27
YCTung 0:830ddddc129f 64 {5.91 ,57 ,-55.345,13.5}, //28
YCTung 0:830ddddc129f 65 {5.91 ,57 ,-57 ,13}, //29
YCTung 0:830ddddc129f 66 {5.92 ,58 ,-59.09 ,12.94}, //30
YCTung 0:830ddddc129f 67 {5.94 ,58 ,-60 ,11.5}, //31
YCTung 0:830ddddc129f 68 {5.95 ,59 ,-62 ,10.5}, //32
YCTung 0:830ddddc129f 69 {5.96 ,60 ,-65 ,11}, //33
YCTung 0:830ddddc129f 70 {5.97 ,62.5 ,-69 ,11.75}, //34
YCTung 0:830ddddc129f 71 {5.98 ,65.6 ,-75.77 ,12}, //35
YCTung 0:830ddddc129f 72 {6.07 ,70.7 ,-83 ,15.5}, //36
YCTung 0:830ddddc129f 73 {6.16 ,75.87 ,-92.37 ,20}, //37
YCTung 0:830ddddc129f 74 {6.22 ,83.07 ,-105 ,28}, //38
YCTung 0:830ddddc129f 75 {6.51 ,85.53 ,-105.5 ,24}, //39
YCTung 0:830ddddc129f 76 {6.51 ,89.03 ,-106 ,22}, //40
YCTung 0:830ddddc129f 77 {12.17 ,93.9 ,-106.84,16.5}, //41
YCTung 0:830ddddc129f 78 {12 ,100 ,-108.5 ,13}, //42
YCTung 0:830ddddc129f 79 {12.6 ,108 ,-110 ,10}, //43
YCTung 0:830ddddc129f 80 {12.25 ,112 ,-109.5 ,6.5}, //44
YCTung 0:830ddddc129f 81 {12 ,114 ,-108.87,4}, //45
YCTung 0:830ddddc129f 82 {12.45 ,115 ,-106.1 ,2}, //46
YCTung 0:830ddddc129f 83 {8.9 ,115.5 ,-101.5 ,-3.5}}; //47
YCTung 0:830ddddc129f 84
YCTung 0:830ddddc129f 85 const float footangle_right[TOT_FOOT_POS][4] = {
YCTung 0:830ddddc129f 86 {-5.92 ,-63 ,42 ,-4.5}, //0
YCTung 0:830ddddc129f 87 {-5.91 ,-62.5 ,44 ,-6}, //1
YCTung 0:830ddddc129f 88 {-5.91 ,-61.5 ,45 ,-7}, //2
YCTung 0:830ddddc129f 89 {-5.91 ,-60 ,47.95 ,-8}, //3
YCTung 0:830ddddc129f 90 {-5.91 ,-61.5 ,52.19 ,-11}, //4
YCTung 0:830ddddc129f 91 {-6.01 ,-62.04 ,58.71 ,-14.265}, //5
YCTung 0:830ddddc129f 92 {-6.11 ,-65.68 ,65.24 ,-15.15}, //6
YCTung 0:830ddddc129f 93 {-6.11 ,-46.68 ,30.24 ,14.15}, //(-6.11 ,-65.68^^^ ,65.24vvv ,-15.15^^^)s1 used for stop mode (from 6)
YCTung 0:830ddddc129f 94 {-6.2 ,-45.51 ,30.1 ,15}, //(-6.2 ,-69.51^^^ ,75.1vvv ,-20^^^)s2 used for stop mode (from 7)
YCTung 0:830ddddc129f 95 {-6.47 ,-45.34 ,30.5 ,18.12}, //(-6.47 ,-73.34 ,84.5 ,-23.12)s3 used for stop mode (from 8)
YCTung 0:830ddddc129f 96 {-6.47 ,-45.29 ,30.5 ,24.79}, //(-6.47 ,-75.29 ,86.5 ,-20.79)s4 used for stop mode (from 9)
YCTung 0:830ddddc129f 97 {-6.47 ,-42.245,25 ,30.475}, //(-6.47 ,-77.245,88 ,-15.475)s5 used for stop mode (from 10)
YCTung 0:830ddddc129f 98 {-6.61 ,-37.15 ,17.56 ,41.7}, //(-6.61 ,-81.15 ,92.56 ,-15.7)s6 used for stop mode (from 11)
YCTung 0:830ddddc129f 99 {-6.61 ,-34.65 ,11.41 ,49.492}, //(-6.61 ,-83.65 ,96.41 ,-16.492)s7 used for stop mode (from 12)
YCTung 0:830ddddc129f 100 {-6.2 ,-69.51 ,75.1 ,-20}, //7
YCTung 0:830ddddc129f 101 {-6.47 ,-73.34 ,84.5 ,-23.12}, //8
YCTung 0:830ddddc129f 102 {-6.47 ,-75.29 ,86.5 ,-20.79}, //9
YCTung 0:830ddddc129f 103 {-6.47 ,-77.245,88 ,-15.475}, //10
YCTung 0:830ddddc129f 104 {-6.61 ,-81.15 ,92.56 ,-15.7}, //11
YCTung 0:830ddddc129f 105 {-6.61 ,-83.65 ,96.41 ,-16.492}, //12
YCTung 0:830ddddc129f 106 {-6.61 ,-86.15 ,97.5 ,-13.67}, //13
YCTung 0:830ddddc129f 107 {-6.7 ,-88.23 ,99 ,-13}, //14
YCTung 0:830ddddc129f 108 {-6.79 ,-90.31 ,98 ,-7}, //15
YCTung 0:830ddddc129f 109 {-6.83 ,-91 ,97.5 ,-3}, //16
YCTung 0:830ddddc129f 110 {-6.88 ,-92.395,95.035 ,2.5}, //17
YCTung 0:830ddddc129f 111 {-7.58 ,-94.28 ,92.84 ,9.435}, //18
YCTung 0:830ddddc129f 112 {-8.28 ,-96.17 ,88.66 ,17.54}, //19
YCTung 0:830ddddc129f 113 {-8.66 ,-98.52 ,84.26 ,25.07}, //20
YCTung 0:830ddddc129f 114 {-9.05 ,-102.88,83.73 ,28.61}, //21
YCTung 0:830ddddc129f 115 {-9.09 ,-100.88,78.3 ,33.49}, //22
YCTung 0:830ddddc129f 116 {-9.14 ,-101.75,72.9 ,39.57}, //23
YCTung 0:830ddddc129f 117 {-9.19 ,-101.75,69.05 ,41.25}, //24
YCTung 0:830ddddc129f 118 {-9.24 ,-102.5 ,67.2 ,43.14}, //25
YCTung 0:830ddddc129f 119 {-9.25 ,-104 ,67 ,41.13}, //26
YCTung 0:830ddddc129f 120 {-9.26 ,-108 ,64.59 ,39.35}, //27
YCTung 0:830ddddc129f 121 {-9.26 ,-109.5 ,65.59 ,37.27}, //28
YCTung 0:830ddddc129f 122 {-9.27 ,-110 ,66.59 ,31.41}, //29
YCTung 0:830ddddc129f 123 {-9.2 ,-110 ,66.59 ,28.55}, //30
YCTung 0:830ddddc129f 124 {-9.2 ,-112 ,69.4 ,21.7}, //31
YCTung 0:830ddddc129f 125 {-9.16 ,-111 ,69.7 ,18.48}, //32
YCTung 0:830ddddc129f 126 {-9.13 ,-111 ,68.5 ,15.27}, //33
YCTung 0:830ddddc129f 127 {-8.75 ,-106.17,62.54 ,18.52}, //34
YCTung 0:830ddddc129f 128 {-8.65 ,-103 ,60.3 ,16.68}, //35
YCTung 0:830ddddc129f 129 {-8.02 ,-101.3 ,60.85 ,12.25}, //36
YCTung 0:830ddddc129f 130 {-7.76 ,-100.64,63.41 ,5.18}, //37
YCTung 0:830ddddc129f 131 {-7.64 ,-97.59 ,62.07 ,1.61}, //38
YCTung 0:830ddddc129f 132 {-7.53 ,-94.54 ,60.74 ,-1.04}, //39
YCTung 0:830ddddc129f 133 {-7.42 ,-92.74 ,60.1 ,-3.15}, //40
YCTung 0:830ddddc129f 134 {-7.315 ,-89.8 ,63.4 ,-9.275}, //41
YCTung 0:830ddddc129f 135 {-6.41 ,-84.63 ,63.1 ,-14.99}, //42
YCTung 0:830ddddc129f 136 {-6.35 ,-82.51 ,62 ,-17.5}, //43
YCTung 0:830ddddc129f 137 {-6.24 ,-78.07 ,62 ,-18.8}, //44
YCTung 0:830ddddc129f 138 {-6.19 ,-72.945,54 ,-10.77}, //45
YCTung 0:830ddddc129f 139 {-6.14 ,-68 ,46 ,-5.7}, //46
YCTung 0:830ddddc129f 140 {-6.09 ,-64.985,41 ,-4.5}}; //47
YCTung 0:830ddddc129f 141
YCTung 0:830ddddc129f 142 extern void setupServo(void){
YCTung 0:830ddddc129f 143 pwm_lw.period(PWM_PERIOD);pwm_lw.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 144 pwm_le.period(PWM_PERIOD);pwm_le.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 145 pwm_ls.period(PWM_PERIOD);pwm_ls.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 146 pwm_la.period(PWM_PERIOD);pwm_la.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 147 pwm_lk.period(PWM_PERIOD);pwm_lk.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 148 pwm_ll.period(PWM_PERIOD);pwm_ll.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 149 pwm_lb.period(PWM_PERIOD);pwm_lb.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 150 pwm_rb.period(PWM_PERIOD);pwm_rb.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 151 pwm_rl.period(PWM_PERIOD);pwm_rl.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 152 pwm_rk.period(PWM_PERIOD);pwm_rk.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 153 pwm_ra.period(PWM_PERIOD);pwm_ra.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 154 pwm_rs.period(PWM_PERIOD);pwm_rs.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 155 pwm_re.period(PWM_PERIOD);pwm_re.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 156 pwm_rw.period(PWM_PERIOD);pwm_rw.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 157 }
YCTung 0:830ddddc129f 158
YCTung 0:830ddddc129f 159 void pwm_left(float angle_up, float angle_mid, float angle_down){ //Left arm
YCTung 0:830ddddc129f 160 pwm_ls.pulsewidth((angle_up+10.0f) / 90.0f * 0.0008f + 0.002297f);
YCTung 0:830ddddc129f 161 pwm_le.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0007044f);
YCTung 0:830ddddc129f 162 pwm_lw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
YCTung 0:830ddddc129f 163 // OCR3B = 574.0+(400.0/180.0)*(angle_up+10); //pin 2
YCTung 0:830ddddc129f 164 // OCR2B = 44.0+(100.0/180.0)*angle_mid; //pin 9
YCTung 0:830ddddc129f 165 // OCR0B = 94+(100.0/180.0)*(-1)*angle_down; // pin 4
YCTung 0:830ddddc129f 166 }
YCTung 0:830ddddc129f 167 void pwm_right(float angle_up, float angle_mid, float angle_down){ //Right arm
YCTung 0:830ddddc129f 168 pwm_rs.pulsewidth((angle_up-3.0f) / 90.0f * 0.0008f + 0.0007f);
YCTung 0:830ddddc129f 169 pwm_re.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0023f);
YCTung 0:830ddddc129f 170 pwm_rw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
YCTung 0:830ddddc129f 171 // OCR1B = 175+(400.0/180.0)*(angle_up-3); //pin 12
YCTung 0:830ddddc129f 172 // OCR2A = 144.0+(100.0/180.0)*angle_mid; //pin 10
YCTung 0:830ddddc129f 173 // OCR0A = 94+(100.0/180.0)*(-1)*angle_down; // pin 13
YCTung 0:830ddddc129f 174 }
YCTung 0:830ddddc129f 175 void lookuptable_steering(int8_t sensor_output){
YCTung 0:830ddddc129f 176 int i = sensor_output/2;
YCTung 0:830ddddc129f 177 int q1 = servoangle_left[20+i][0];
YCTung 0:830ddddc129f 178 int q2 = servoangle_left[21+i][0];
YCTung 0:830ddddc129f 179 int q3 = servoangle_left[20+i][1];
YCTung 0:830ddddc129f 180 int q4 = servoangle_left[21+i][1];
YCTung 0:830ddddc129f 181 int q5 = servoangle_left[20+i][2];
YCTung 0:830ddddc129f 182 int q6 = servoangle_left[21+i][2];
YCTung 0:830ddddc129f 183 int q7 = servoangle_right[20+i][0];
YCTung 0:830ddddc129f 184 int q8 = servoangle_right[21+i][0];
YCTung 0:830ddddc129f 185 int q9 = servoangle_right[20+i][1];
YCTung 0:830ddddc129f 186 int q10 = servoangle_right[21+i][1];
YCTung 0:830ddddc129f 187 int q11 = servoangle_right[20+i][2];
YCTung 0:830ddddc129f 188 int q12 = servoangle_right[21+i][2];
YCTung 0:830ddddc129f 189 float x_L1 = (q2-q1)*(sensor_output-2*i)/2 + q1;
YCTung 0:830ddddc129f 190 float x_L2 = (q4-q3)*(sensor_output-2*i)/2 + q3;
YCTung 0:830ddddc129f 191 float x_L3 = (q6-q5)*(sensor_output-2*i)/2 + q5;
YCTung 0:830ddddc129f 192 float x_R1 = (q8-q7)*(sensor_output-2*i)/2 + q7;
YCTung 0:830ddddc129f 193 float x_R2 = (q10-q9)*(sensor_output-2*i)/2 + q9;
YCTung 0:830ddddc129f 194 float x_R3 = (q12-q11)*(sensor_output-2*i)/2 + q11;
YCTung 0:830ddddc129f 195 pwm_left(x_L1,x_L2,x_L3);
YCTung 0:830ddddc129f 196 pwm_right(x_R1,x_R2,x_R3);
YCTung 0:830ddddc129f 197 }
YCTung 0:830ddddc129f 198
YCTung 0:830ddddc129f 199 void pwm_left_foot(float angle_1,float angle_2,float angle_3,float angle_4){
YCTung 0:830ddddc129f 200 pwm_lb.pulsewidth(POWER_REDU_PW);
YCTung 0:830ddddc129f 201 pwm_ll.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.0007f);
YCTung 0:830ddddc129f 202 pwm_lk.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.002297f);
YCTung 0:830ddddc129f 203 pwm_la.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
YCTung 0:830ddddc129f 204 // OCR4A = 25;//(-(angle_1 + 4.0) / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6
YCTung 0:830ddddc129f 205 // OCR3A = 175 + (400/180)*angle_2; // Pin 5
YCTung 0:830ddddc129f 206 // OCR3C = 574 + (400/180)*angle_3; // Pin 3
YCTung 0:830ddddc129f 207 // OCR5B = 378 + (400/180)*angle_4*(-1); // Pin 45
YCTung 0:830ddddc129f 208 }
YCTung 0:830ddddc129f 209 void pwm_right_foot(float angle_1,float angle_2,float angle_3,float angle_4){
YCTung 0:830ddddc129f 210 pwm_rb.pulsewidth(8.5f / 90.0f * 0.0008f + 0.0015f);
YCTung 0:830ddddc129f 211 pwm_rb.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.002297f);
YCTung 0:830ddddc129f 212 pwm_rb.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.0007f);
YCTung 0:830ddddc129f 213 pwm_rb.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
YCTung 0:830ddddc129f 214 // OCR4B = (8.5 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 7
YCTung 0:830ddddc129f 215 // OCR4C = 574 + (400/180)*angle_2; // Pin 8
YCTung 0:830ddddc129f 216 // OCR1A = 175 + (400/180)*angle_3; // Pin 11
YCTung 0:830ddddc129f 217 // OCR5A = 378 + (400/180)*angle_4*(-1); // Pin 46
YCTung 0:830ddddc129f 218 }
YCTung 0:830ddddc129f 219 void lookuptable_pedaling(int index){
YCTung 0:830ddddc129f 220 int q0 = footangle_left[index][0];
YCTung 0:830ddddc129f 221 int q1 = footangle_left[index][1];
YCTung 0:830ddddc129f 222 int q2 = footangle_left[index][2];
YCTung 0:830ddddc129f 223 int q3 = footangle_left[index][3];
YCTung 0:830ddddc129f 224 int q4 = footangle_right[index][0];
YCTung 0:830ddddc129f 225 int q5 = footangle_right[index][1];
YCTung 0:830ddddc129f 226 int q6 = footangle_right[index][2];
YCTung 0:830ddddc129f 227 int q7 = footangle_right[index][3];
YCTung 0:830ddddc129f 228 pwm_left_foot(q0,q1,q2,q3);
YCTung 0:830ddddc129f 229 pwm_right_foot(q4,q5,q6,q7);
YCTung 0:830ddddc129f 230 }
YCTung 0:830ddddc129f 231 void reset_left_butt(void)
YCTung 0:830ddddc129f 232 {
YCTung 0:830ddddc129f 233 pwm_lb.pulsewidth(-12.0f / 90.0f * 0.0008f + 0.0015f);
YCTung 0:830ddddc129f 234 // OCR4A = (-12.0 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6, left butt
YCTung 0:830ddddc129f 235 }
YCTung 0:830ddddc129f 236 void reset_pos(void)
YCTung 0:830ddddc129f 237 {
YCTung 0:830ddddc129f 238 pwm_left_foot(8.86 ,115 ,-80 ,-12.739);
YCTung 0:830ddddc129f 239 reset_left_butt();
YCTung 0:830ddddc129f 240 pwm_right_foot(-6.11 ,-48.68 ,32.24 ,12.15);
YCTung 0:830ddddc129f 241 // pwm_right_foot(-6.11 ,-50.68 ,38.24 ,4.15);
YCTung 0:830ddddc129f 242 }
YCTung 0:830ddddc129f 243 void stop_pos(void)
YCTung 0:830ddddc129f 244 {
YCTung 0:830ddddc129f 245 pwm_left_foot(8.86 ,115 ,-80 ,-12.739);
YCTung 0:830ddddc129f 246 reset_left_butt();
YCTung 0:830ddddc129f 247 // pwm_right_foot(-6.11 ,-48.68 ,32.24 ,12.15);
YCTung 0:830ddddc129f 248 pwm_right_foot(-6.11 ,-50.68 ,38.24 ,4.15);
YCTung 0:830ddddc129f 249 }
YCTung 0:830ddddc129f 250 int angle_to_duty(int resolution, float degree)
YCTung 0:830ddddc129f 251 {//+-90 degrees
YCTung 1:709be64ca63c 252 return (int)((degree / 90.0f * 0.0008f + 0.0015f) * 244.0f * resolution);
YCTung 0:830ddddc129f 253 }