![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
main.cpp@4:a59512fe0f9a, 2018-08-27 (annotated)
- Committer:
- JJting
- Date:
- Mon Aug 27 15:36:28 2018 +0000
- Revision:
- 4:a59512fe0f9a
- Parent:
- 3:c05acc05c3bd
- Child:
- 5:131450b16ce3
????OK PID = 7.2 0 1.3
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JJting | 0:c23e915f255b | 1 | #include "mbed.h" |
JJting | 0:c23e915f255b | 2 | #include "encoder.h" |
JJting | 0:c23e915f255b | 3 | #include "Mx28.h" |
JJting | 0:c23e915f255b | 4 | #include "PID.h" |
JJting | 0:c23e915f255b | 5 | |
JJting | 0:c23e915f255b | 6 | //********************* Dynamxiel ****************************** |
JJting | 0:c23e915f255b | 7 | #define SERVO_ID 0x01 // ID of which we will set Dynamixel too |
JJting | 0:c23e915f255b | 8 | #define SERVO_ControlPin A2 // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer. |
JJting | 0:c23e915f255b | 9 | #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps) |
JJting | 0:c23e915f255b | 10 | #define TxPin A0 |
JJting | 0:c23e915f255b | 11 | #define RxPin A1 |
JJting | 0:c23e915f255b | 12 | #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode |
JJting | 0:c23e915f255b | 13 | #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode |
JJting | 0:c23e915f255b | 14 | #define PI 3.14159265f |
JJting | 0:c23e915f255b | 15 | //*************************************************************** |
JJting | 0:c23e915f255b | 16 | |
JJting | 0:c23e915f255b | 17 | Serial uart(USBTX, USBRX); |
JJting | 0:c23e915f255b | 18 | //Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth |
JJting | 0:c23e915f255b | 19 | DigitalOut LED(A4); // check if the code is running |
JJting | 0:c23e915f255b | 20 | DigitalOut led2(A5); // check if the code is running interrupt |
JJting | 0:c23e915f255b | 21 | uint8_t led2f; |
JJting | 0:c23e915f255b | 22 | |
JJting | 0:c23e915f255b | 23 | // Timer |
JJting | 0:c23e915f255b | 24 | Ticker timer1; |
JJting | 1:2823a39f70a9 | 25 | float ITR_time1 = 4000.0; // unit:us |
JJting | 0:c23e915f255b | 26 | float Ts = ITR_time1/1000000; |
JJting | 0:c23e915f255b | 27 | uint8_t flag; |
JJting | 0:c23e915f255b | 28 | |
JJting | 0:c23e915f255b | 29 | // uart_tx |
JJting | 0:c23e915f255b | 30 | union splitter { |
JJting | 0:c23e915f255b | 31 | short j; |
JJting | 0:c23e915f255b | 32 | char C[2]; |
JJting | 0:c23e915f255b | 33 | // C[0] is lowbyte of j, C[1] is highbyte of j |
JJting | 0:c23e915f255b | 34 | }; |
JJting | 0:c23e915f255b | 35 | char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; |
JJting | 0:c23e915f255b | 36 | int i = 0; |
JJting | 0:c23e915f255b | 37 | |
JJting | 0:c23e915f255b | 38 | // PID |
JJting | 4:a59512fe0f9a | 39 | PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 |
JJting | 0:c23e915f255b | 40 | float PIDout = 0.0f; |
JJting | 0:c23e915f255b | 41 | |
JJting | 0:c23e915f255b | 42 | // Dynamixel |
JJting | 0:c23e915f255b | 43 | DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); |
JJting | 0:c23e915f255b | 44 | int servo_cmd; |
JJting | 1:2823a39f70a9 | 45 | int row_cmd; |
JJting | 0:c23e915f255b | 46 | int D_angle = 0; |
JJting | 0:c23e915f255b | 47 | int D_angle_dif = 0; |
JJting | 0:c23e915f255b | 48 | int D_Angle; |
JJting | 0:c23e915f255b | 49 | int D_angle_old; |
JJting | 0:c23e915f255b | 50 | unsigned short d = 0; |
JJting | 0:c23e915f255b | 51 | // Find Torque |
JJting | 0:c23e915f255b | 52 | double angle_difference = 0.0; |
JJting | 0:c23e915f255b | 53 | float torque_measured = 0.0; |
JJting | 0:c23e915f255b | 54 | float ks = 2.6393*2; //spring constant |
JJting | 0:c23e915f255b | 55 | //int angle_dif = 0; |
JJting | 0:c23e915f255b | 56 | float torque_ref = 0.0; |
JJting | 1:2823a39f70a9 | 57 | //float friction = 0.0f; |
JJting | 0:c23e915f255b | 58 | float friction = 0.18f; |
JJting | 1:2823a39f70a9 | 59 | float rate = 0.5; |
JJting | 0:c23e915f255b | 60 | //float friction = 0.0f; |
JJting | 0:c23e915f255b | 61 | //float check = 0.0f; |
JJting | 1:2823a39f70a9 | 62 | float Angle_Dif; |
JJting | 4:a59512fe0f9a | 63 | short rotation_; |
JJting | 0:c23e915f255b | 64 | |
JJting | 0:c23e915f255b | 65 | // function |
JJting | 0:c23e915f255b | 66 | void init_UART(); |
JJting | 0:c23e915f255b | 67 | void init_TIMER(); |
JJting | 0:c23e915f255b | 68 | void timer1_ITR(); |
JJting | 0:c23e915f255b | 69 | void uart_tx(); |
JJting | 0:c23e915f255b | 70 | |
JJting | 0:c23e915f255b | 71 | void init_DYNAMIXEL(); |
JJting | 0:c23e915f255b | 72 | void D_angle_measure(); |
JJting | 0:c23e915f255b | 73 | void find_torque(); |
JJting | 0:c23e915f255b | 74 | |
JJting | 0:c23e915f255b | 75 | int main() |
JJting | 0:c23e915f255b | 76 | { |
JJting | 0:c23e915f255b | 77 | LED = 1; // darken |
JJting | 0:c23e915f255b | 78 | wait_ms(500); |
JJting | 0:c23e915f255b | 79 | // initial sensor |
JJting | 0:c23e915f255b | 80 | init_SPI_encoder(); |
JJting | 0:c23e915f255b | 81 | init_encoder(); |
JJting | 0:c23e915f255b | 82 | init_DYNAMIXEL(); |
JJting | 0:c23e915f255b | 83 | // initial uart |
JJting | 0:c23e915f255b | 84 | init_UART(); |
JJting | 0:c23e915f255b | 85 | |
JJting | 0:c23e915f255b | 86 | wait_ms(500); |
JJting | 0:c23e915f255b | 87 | |
JJting | 0:c23e915f255b | 88 | led2 = 1; |
JJting | 1:2823a39f70a9 | 89 | // led2f = 0; |
JJting | 0:c23e915f255b | 90 | LED = 0; // lighten |
JJting | 0:c23e915f255b | 91 | |
JJting | 0:c23e915f255b | 92 | init_TIMER(); |
JJting | 0:c23e915f255b | 93 | |
JJting | 0:c23e915f255b | 94 | while(1) { |
JJting | 0:c23e915f255b | 95 | |
JJting | 0:c23e915f255b | 96 | } |
JJting | 0:c23e915f255b | 97 | } |
JJting | 0:c23e915f255b | 98 | |
JJting | 0:c23e915f255b | 99 | void init_DYNAMIXEL() |
JJting | 0:c23e915f255b | 100 | { |
JJting | 0:c23e915f255b | 101 | dynamixelClass.torqueMode(SERVO_ID, 1); |
JJting | 3:c05acc05c3bd | 102 | // dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); |
JJting | 0:c23e915f255b | 103 | wait_ms(1); |
JJting | 0:c23e915f255b | 104 | } |
JJting | 0:c23e915f255b | 105 | |
JJting | 0:c23e915f255b | 106 | void init_UART() |
JJting | 0:c23e915f255b | 107 | { |
JJting | 0:c23e915f255b | 108 | uart.baud(115200); |
JJting | 0:c23e915f255b | 109 | } |
JJting | 0:c23e915f255b | 110 | |
JJting | 0:c23e915f255b | 111 | void init_TIMER() |
JJting | 0:c23e915f255b | 112 | { |
JJting | 0:c23e915f255b | 113 | timer1.attach_us(&timer1_ITR, ITR_time1); |
JJting | 0:c23e915f255b | 114 | } |
JJting | 0:c23e915f255b | 115 | |
JJting | 0:c23e915f255b | 116 | void timer1_ITR() |
JJting | 0:c23e915f255b | 117 | { |
JJting | 3:c05acc05c3bd | 118 | led2 = !led2; |
JJting | 3:c05acc05c3bd | 119 | angle_measure(); |
JJting | 3:c05acc05c3bd | 120 | D_angle_measure(); |
JJting | 3:c05acc05c3bd | 121 | find_torque(); |
JJting | 3:c05acc05c3bd | 122 | motor_pid.Compute(torque_ref, torque_measured); |
JJting | 3:c05acc05c3bd | 123 | PIDout = motor_pid.output; |
JJting | 3:c05acc05c3bd | 124 | servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 |
JJting | 3:c05acc05c3bd | 125 | |
JJting | 4:a59512fe0f9a | 126 | // 電流控制 |
JJting | 3:c05acc05c3bd | 127 | if (servo_cmd > 0) { |
JJting | 4:a59512fe0f9a | 128 | // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; |
JJting | 3:c05acc05c3bd | 129 | servo_cmd = servo_cmd; |
JJting | 3:c05acc05c3bd | 130 | if (servo_cmd >= 1023) |
JJting | 3:c05acc05c3bd | 131 | servo_cmd = 1023; |
JJting | 3:c05acc05c3bd | 132 | } else { |
JJting | 4:a59512fe0f9a | 133 | // servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; |
JJting | 3:c05acc05c3bd | 134 | servo_cmd = -servo_cmd + 1024; |
JJting | 3:c05acc05c3bd | 135 | if (servo_cmd >= 2047) |
JJting | 3:c05acc05c3bd | 136 | servo_cmd = 2047; |
JJting | 3:c05acc05c3bd | 137 | } |
JJting | 3:c05acc05c3bd | 138 | |
JJting | 3:c05acc05c3bd | 139 | if (servo_cmd >= 1023) { |
JJting | 3:c05acc05c3bd | 140 | row_cmd = -(servo_cmd-1023); |
JJting | 3:c05acc05c3bd | 141 | } else { |
JJting | 3:c05acc05c3bd | 142 | row_cmd = servo_cmd; |
JJting | 3:c05acc05c3bd | 143 | } |
JJting | 4:a59512fe0f9a | 144 | /* |
JJting | 4:a59512fe0f9a | 145 | // 速度控制 |
JJting | 4:a59512fe0f9a | 146 | if (servo_cmd > 0) { |
JJting | 4:a59512fe0f9a | 147 | row_cmd = servo_cmd; |
JJting | 4:a59512fe0f9a | 148 | // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f; |
JJting | 4:a59512fe0f9a | 149 | servo_cmd = servo_cmd; |
JJting | 4:a59512fe0f9a | 150 | rotation_ = 0; // 0:Move Left |
JJting | 4:a59512fe0f9a | 151 | if (servo_cmd >= 1023) { |
JJting | 4:a59512fe0f9a | 152 | servo_cmd = 1023; |
JJting | 4:a59512fe0f9a | 153 | row_cmd = servo_cmd; |
JJting | 4:a59512fe0f9a | 154 | } |
JJting | 4:a59512fe0f9a | 155 | } else { |
JJting | 4:a59512fe0f9a | 156 | row_cmd = servo_cmd; |
JJting | 4:a59512fe0f9a | 157 | // servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f; |
JJting | 4:a59512fe0f9a | 158 | servo_cmd = -servo_cmd; |
JJting | 4:a59512fe0f9a | 159 | rotation_ = 1; // 1:Move Right |
JJting | 4:a59512fe0f9a | 160 | if (servo_cmd >= 1023) { |
JJting | 4:a59512fe0f9a | 161 | servo_cmd = 1023; |
JJting | 4:a59512fe0f9a | 162 | row_cmd = -servo_cmd; |
JJting | 4:a59512fe0f9a | 163 | } |
JJting | 4:a59512fe0f9a | 164 | } |
JJting | 4:a59512fe0f9a | 165 | */ |
JJting | 3:c05acc05c3bd | 166 | dynamixelClass.torque(SERVO_ID, servo_cmd); |
JJting | 4:a59512fe0f9a | 167 | // dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) |
JJting | 3:c05acc05c3bd | 168 | // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation) |
JJting | 3:c05acc05c3bd | 169 | uart_tx(); |
JJting | 0:c23e915f255b | 170 | } |
JJting | 0:c23e915f255b | 171 | |
JJting | 0:c23e915f255b | 172 | void uart_tx() |
JJting | 0:c23e915f255b | 173 | { |
JJting | 0:c23e915f255b | 174 | splitter s1; |
JJting | 0:c23e915f255b | 175 | splitter s2; |
JJting | 0:c23e915f255b | 176 | splitter s3; |
JJting | 0:c23e915f255b | 177 | splitter s4; |
JJting | 0:c23e915f255b | 178 | splitter s5; |
JJting | 0:c23e915f255b | 179 | splitter s6; |
JJting | 0:c23e915f255b | 180 | splitter s7; |
JJting | 0:c23e915f255b | 181 | |
JJting | 4:a59512fe0f9a | 182 | s1.j = torque_ref*1000; |
JJting | 4:a59512fe0f9a | 183 | s2.j = torque_measured*1000; |
JJting | 4:a59512fe0f9a | 184 | s3.j = Angle_Dif/4096*3600; |
JJting | 1:2823a39f70a9 | 185 | // s3.j = servo_cmd; |
JJting | 0:c23e915f255b | 186 | // s4.j = 1; |
JJting | 0:c23e915f255b | 187 | // s5.j = 3; |
JJting | 3:c05acc05c3bd | 188 | s4.j = D_angle; |
JJting | 4:a59512fe0f9a | 189 | s5.j = Angle*3; |
JJting | 1:2823a39f70a9 | 190 | s6.j = row_cmd; |
JJting | 1:2823a39f70a9 | 191 | s7.j = 1; |
JJting | 0:c23e915f255b | 192 | |
JJting | 0:c23e915f255b | 193 | T[2] = s1.C[0]; |
JJting | 0:c23e915f255b | 194 | T[3] = s1.C[1]; |
JJting | 0:c23e915f255b | 195 | T[4] = s2.C[0]; |
JJting | 0:c23e915f255b | 196 | T[5] = s2.C[1]; |
JJting | 0:c23e915f255b | 197 | T[6] = s3.C[0]; |
JJting | 0:c23e915f255b | 198 | T[7] = s3.C[1]; |
JJting | 0:c23e915f255b | 199 | T[8] = s4.C[0]; |
JJting | 0:c23e915f255b | 200 | T[9] = s4.C[1]; |
JJting | 0:c23e915f255b | 201 | T[10] = s5.C[0]; |
JJting | 0:c23e915f255b | 202 | T[11] = s5.C[1]; |
JJting | 0:c23e915f255b | 203 | T[12] = s6.C[0]; |
JJting | 0:c23e915f255b | 204 | T[13] = s6.C[1]; |
JJting | 0:c23e915f255b | 205 | T[14] = s7.C[0]; |
JJting | 0:c23e915f255b | 206 | T[15] = s7.C[1]; |
JJting | 0:c23e915f255b | 207 | |
JJting | 0:c23e915f255b | 208 | while(1) { |
JJting | 0:c23e915f255b | 209 | if (uart.writeable() == 1) { |
JJting | 0:c23e915f255b | 210 | uart.putc(T[i]); |
JJting | 0:c23e915f255b | 211 | i++; |
JJting | 0:c23e915f255b | 212 | } |
JJting | 1:2823a39f70a9 | 213 | if (i >= sizeof(T)) { |
JJting | 0:c23e915f255b | 214 | i = 0; |
JJting | 0:c23e915f255b | 215 | break; |
JJting | 0:c23e915f255b | 216 | } |
JJting | 0:c23e915f255b | 217 | } |
JJting | 0:c23e915f255b | 218 | } |
JJting | 0:c23e915f255b | 219 | |
JJting | 0:c23e915f255b | 220 | void D_angle_measure() |
JJting | 0:c23e915f255b | 221 | { |
JJting | 0:c23e915f255b | 222 | D_angle = dynamixelClass.readPosition(SERVO_ID); |
JJting | 0:c23e915f255b | 223 | |
JJting | 0:c23e915f255b | 224 | if (d == 0) { |
JJting | 0:c23e915f255b | 225 | D_Angle = 0; |
JJting | 0:c23e915f255b | 226 | D_angle_old = D_angle; |
JJting | 0:c23e915f255b | 227 | d++; |
JJting | 0:c23e915f255b | 228 | } else { |
JJting | 0:c23e915f255b | 229 | D_angle_dif = D_angle - D_angle_old; |
JJting | 3:c05acc05c3bd | 230 | |
JJting | 3:c05acc05c3bd | 231 | if (D_angle_dif > 4096/2) { |
JJting | 3:c05acc05c3bd | 232 | D_angle_dif = -(4096-(D_angle_dif)); |
JJting | 3:c05acc05c3bd | 233 | } else if (D_angle_dif < -4096/2) { |
JJting | 3:c05acc05c3bd | 234 | D_angle_dif = -(-4096-(D_angle_dif)); |
JJting | 3:c05acc05c3bd | 235 | } else { |
JJting | 3:c05acc05c3bd | 236 | D_angle_dif = D_angle_dif; |
JJting | 3:c05acc05c3bd | 237 | } |
JJting | 3:c05acc05c3bd | 238 | |
JJting | 0:c23e915f255b | 239 | D_Angle = D_Angle + D_angle_dif; |
JJting | 0:c23e915f255b | 240 | D_angle_old = D_angle; |
JJting | 0:c23e915f255b | 241 | } |
JJting | 0:c23e915f255b | 242 | } |
JJting | 0:c23e915f255b | 243 | |
JJting | 0:c23e915f255b | 244 | void find_torque() |
JJting | 0:c23e915f255b | 245 | { |
JJting | 1:2823a39f70a9 | 246 | |
JJting | 3:c05acc05c3bd | 247 | Angle_Dif = Angle*3-D_Angle; |
JJting | 3:c05acc05c3bd | 248 | angle_difference = Angle_Dif/4096.0f*2*PI; |
JJting | 1:2823a39f70a9 | 249 | // angle_difference = Angle_Dif/4096.0f*2*PI; |
JJting | 1:2823a39f70a9 | 250 | |
JJting | 0:c23e915f255b | 251 | torque_measured = angle_difference*ks; |
JJting | 1:2823a39f70a9 | 252 | } |