Chen Wei Ting / Mbed 2 deprecated zerotorque_final

zero torque and encoder

Dependencies:   MX28 PID mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

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?

UserRevisionLine numberNew 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 }