Chris LU / Mbed 2 deprecated Nucleo_test

new robot bike code

Dependencies:   SDFileSystem mbed

RobotServo.cpp@0:dd11d16436bf, 2017-02-22 (annotated)

Committer:
cpul5338
Date:
Wed Feb 22 14:38:27 2017 +0000
Revision:
0:dd11d16436bf
123

Who changed what in which revision?

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