Rong Syuan Lin / Mbed 2 deprecated LSM9DS1_project_5_zerotorque

20181105

Dependencies:   MX28 mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

main.cpp@8:9c3b291b3288, 2018-08-14 (annotated)

Committer:
JJting
Date:
Tue Aug 14 06:22:27 2018 +0000
Revision:
8:9c3b291b3288
Parent:
7:5cb292622961
Child:
9:07de3af99031
0-torque

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 4:f19e7669d1b5 5 #include "LSM9DS1.h"
JJting 0:c23e915f255b 6
JJting 0:c23e915f255b 7 //********************* Dynamxiel ******************************
JJting 0:c23e915f255b 8 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
JJting 0:c23e915f255b 9 #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 10 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
JJting 0:c23e915f255b 11 #define TxPin A0
JJting 0:c23e915f255b 12 #define RxPin A1
JJting 0:c23e915f255b 13 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
JJting 0:c23e915f255b 14 #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
JJting 4:f19e7669d1b5 15 #define _PI 3.14159265f
JJting 0:c23e915f255b 16 //***************************************************************
JJting 0:c23e915f255b 17
JJting 0:c23e915f255b 18 Serial uart(USBTX, USBRX);
JJting 0:c23e915f255b 19 //Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth
JJting 0:c23e915f255b 20 DigitalOut LED(A4); // check if the code is running
JJting 0:c23e915f255b 21 DigitalOut led2(A5); // check if the code is running interrupt
JJting 0:c23e915f255b 22 uint8_t led2f;
JJting 3:98cdee5d9b63 23 AnalogIn EMG(PC_4);
JJting 4:f19e7669d1b5 24 DigitalOut test(PC_8);
JJting 4:f19e7669d1b5 25 //AnalogOut test2(PA_5);
JJting 4:f19e7669d1b5 26 // IMU
JJting 4:f19e7669d1b5 27 LSM9DS1 imu(D14, D15); //SDA SCL
JJting 4:f19e7669d1b5 28 LSM9DS1 imu2(PC_9, D7); //SDA SCL
JJting 4:f19e7669d1b5 29 void init_IMU();
JJting 4:f19e7669d1b5 30 int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis
JJting 4:f19e7669d1b5 31 int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis
JJting 4:f19e7669d1b5 32 float Acce_axis_data_f[6] = {0};
JJting 4:f19e7669d1b5 33 float Acce_axis_data_f_old[6] = {0};
JJting 4:f19e7669d1b5 34 float Gyro_axis_data_f[6] = {0};
JJting 4:f19e7669d1b5 35 float Gyro_axis_data_f_old[6] = {0};
JJting 4:f19e7669d1b5 36
JJting 0:c23e915f255b 37
JJting 0:c23e915f255b 38 // Timer
JJting 0:c23e915f255b 39 Ticker timer1;
JJting 4:f19e7669d1b5 40 float ITR_time1 = 6000.0; // unit：us
JJting 0:c23e915f255b 41 float Ts = ITR_time1/1000000;
JJting 0:c23e915f255b 42 uint8_t flag;
JJting 0:c23e915f255b 43
JJting 3:98cdee5d9b63 44 // EMG
JJting 3:98cdee5d9b63 45 float lpf(float input, float output_old, float frequency);
JJting 3:98cdee5d9b63 46 float emg_value;
JJting 3:98cdee5d9b63 47 float emg_value_f;
JJting 3:98cdee5d9b63 48 float emg_value_f_old;
JJting 4:f19e7669d1b5 49 float Tf = ITR_time1*0.000001f;
JJting 3:98cdee5d9b63 50
JJting 0:c23e915f255b 51 // uart_tx
JJting 0:c23e915f255b 52 union splitter {
JJting 0:c23e915f255b 53 short j;
JJting 0:c23e915f255b 54 char C[2];
JJting 0:c23e915f255b 55 // C[0] is lowbyte of j, C[1] is highbyte of j
JJting 0:c23e915f255b 56 };
JJting 0:c23e915f255b 57 char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
JJting 0:c23e915f255b 58 int i = 0;
JJting 0:c23e915f255b 59
JJting 0:c23e915f255b 60 // PID
JJting 8:9c3b291b3288 61 PID motor_pid(5, 0, 0, Ts);// 6.4 0.13 7.2 0.13 4.8, 0.568, 0.142
JJting 0:c23e915f255b 62 float PIDout = 0.0f;
JJting 0:c23e915f255b 63
JJting 0:c23e915f255b 64 // Dynamixel
JJting 0:c23e915f255b 65 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
JJting 0:c23e915f255b 66 int servo_cmd;
JJting 1:2823a39f70a9 67 int row_cmd;
JJting 0:c23e915f255b 68 int D_angle = 0;
JJting 0:c23e915f255b 69 int D_angle_dif = 0;
JJting 0:c23e915f255b 70 int D_Angle;
JJting 0:c23e915f255b 71 int D_angle_old;
JJting 0:c23e915f255b 72 unsigned short d = 0;
JJting 0:c23e915f255b 73 // Find Torque
JJting 4:f19e7669d1b5 74 double _angle_difference = 0.0;
JJting 4:f19e7669d1b5 75 double torque_measured = 0.0;
JJting 0:c23e915f255b 76 float ks = 2.6393*2; //spring constant
JJting 0:c23e915f255b 77 //int angle_dif = 0;
JJting 7:5cb292622961 78 float torque_ref = 0.0f; //*************************************************
JJting 4:f19e7669d1b5 79 float friction = 0.0f;
JJting 7:5cb292622961 80 //float friction = 0.5f;
JJting 4:f19e7669d1b5 81 float rate = 0.8;
JJting 0:c23e915f255b 82 //float friction = 0.0f;
JJting 0:c23e915f255b 83 //float check = 0.0f;
JJting 1:2823a39f70a9 84 float Angle_Dif;
JJting 5:4dbed091ec5a 85 short rotation_;
JJting 0:c23e915f255b 86 // function
JJting 0:c23e915f255b 87 void init_UART();
JJting 0:c23e915f255b 88 void init_TIMER();
JJting 0:c23e915f255b 89 void timer1_ITR();
JJting 0:c23e915f255b 90 void uart_tx();
JJting 0:c23e915f255b 91
JJting 0:c23e915f255b 92 void init_DYNAMIXEL();
JJting 0:c23e915f255b 93 void D_angle_measure();
JJting 0:c23e915f255b 94 void find_torque();
JJting 0:c23e915f255b 95
JJting 0:c23e915f255b 96 int main()
JJting 0:c23e915f255b 97 {
JJting 0:c23e915f255b 98 LED = 1; // darken
JJting 0:c23e915f255b 99 wait_ms(500);
JJting 0:c23e915f255b 100 // initial sensor
JJting 0:c23e915f255b 101 init_SPI_encoder();
JJting 0:c23e915f255b 102 init_encoder();
JJting 4:f19e7669d1b5 103 init_IMU();
JJting 0:c23e915f255b 104 init_DYNAMIXEL();
JJting 0:c23e915f255b 105 // initial uart
JJting 0:c23e915f255b 106 init_UART();
JJting 0:c23e915f255b 107
JJting 0:c23e915f255b 108 wait_ms(500);
JJting 0:c23e915f255b 109
JJting 0:c23e915f255b 110 led2 = 1;
JJting 1:2823a39f70a9 111 // led2f = 0;
JJting 0:c23e915f255b 112 LED = 0; // lighten
JJting 0:c23e915f255b 113
JJting 0:c23e915f255b 114 init_TIMER();
JJting 0:c23e915f255b 115
JJting 0:c23e915f255b 116 while(1) {
JJting 0:c23e915f255b 117
JJting 0:c23e915f255b 118 }
JJting 0:c23e915f255b 119 }
JJting 0:c23e915f255b 120
JJting 4:f19e7669d1b5 121 void init_IMU()
JJting 4:f19e7669d1b5 122 {
JJting 4:f19e7669d1b5 123 imu.begin();
JJting 4:f19e7669d1b5 124 imu2.begin();
JJting 4:f19e7669d1b5 125 }
JJting 4:f19e7669d1b5 126
JJting 0:c23e915f255b 127 void init_DYNAMIXEL()
JJting 0:c23e915f255b 128 {
JJting 5:4dbed091ec5a 129 // dynamixelClass.torqueMode(SERVO_ID, 0);
JJting 5:4dbed091ec5a 130 dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
JJting 5:4dbed091ec5a 131 // dynamixelClass.setPID(SERVO_ID, 1, 0, 0);
JJting 0:c23e915f255b 132 wait_ms(1);
JJting 0:c23e915f255b 133 }
JJting 0:c23e915f255b 134
JJting 0:c23e915f255b 135 void init_UART()
JJting 0:c23e915f255b 136 {
JJting 0:c23e915f255b 137 uart.baud(115200);
JJting 0:c23e915f255b 138 }
JJting 0:c23e915f255b 139
JJting 0:c23e915f255b 140 void init_TIMER()
JJting 0:c23e915f255b 141 {
JJting 0:c23e915f255b 142 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 143 }
JJting 0:c23e915f255b 144
JJting 0:c23e915f255b 145 void timer1_ITR()
JJting 0:c23e915f255b 146 {
JJting 5:4dbed091ec5a 147 test = 1;
JJting 5:4dbed091ec5a 148 led2 = !led2;
JJting 5:4dbed091ec5a 149 angle_measure();
JJting 5:4dbed091ec5a 150 D_angle_measure();
JJting 5:4dbed091ec5a 151 find_torque();
JJting 5:4dbed091ec5a 152 motor_pid.Compute(torque_ref, torque_measured);
JJting 5:4dbed091ec5a 153 PIDout = motor_pid.output;
JJting 7:5cb292622961 154 servo_cmd = PIDout*121.78f; // 1023/8.4Nm = 121.78
JJting 5:4dbed091ec5a 155
JJting 5:4dbed091ec5a 156 if (servo_cmd > 0) {
JJting 7:5cb292622961 157 row_cmd = servo_cmd;
JJting 7:5cb292622961 158 servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f;
JJting 7:5cb292622961 159 // servo_cmd = servo_cmd;
JJting 7:5cb292622961 160 rotation_ = 0; // 0：Move Left
JJting 7:5cb292622961 161 if (servo_cmd >= 1023) {
JJting 5:4dbed091ec5a 162 servo_cmd = 1023;
JJting 7:5cb292622961 163 row_cmd = servo_cmd;
JJting 7:5cb292622961 164 }
JJting 5:4dbed091ec5a 165 } else {
JJting 7:5cb292622961 166 row_cmd = servo_cmd;
JJting 7:5cb292622961 167 servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f;
JJting 7:5cb292622961 168 // servo_cmd = -servo_cmd;
JJting 7:5cb292622961 169 rotation_ = 1; // 1：Move Right
JJting 7:5cb292622961 170 if (servo_cmd >= 1023) {
JJting 5:4dbed091ec5a 171 servo_cmd = 1023;
JJting 7:5cb292622961 172 row_cmd = -servo_cmd;
JJting 7:5cb292622961 173 }
JJting 5:4dbed091ec5a 174 }
JJting 5:4dbed091ec5a 175
JJting 7:5cb292622961 176 // row_cmd = servo_cmd;
JJting 5:4dbed091ec5a 177
JJting 5:4dbed091ec5a 178 // if (servo_cmd > 1023) {
JJting 5:4dbed091ec5a 179 // row_cmd = -(servo_cmd-1023);
JJting 5:4dbed091ec5a 180 // } else {
JJting 5:4dbed091ec5a 181 // row_cmd = servo_cmd;
JJting 5:4dbed091ec5a 182 // }
JJting 5:4dbed091ec5a 183
JJting 5:4dbed091ec5a 184 // EMG
JJting 5:4dbed091ec5a 185 emg_value = EMG.read();
JJting 5:4dbed091ec5a 186 emg_value_f = lpf(emg_value,emg_value_f_old,15);
JJting 5:4dbed091ec5a 187 emg_value_f_old = emg_value_f;
JJting 5:4dbed091ec5a 188
JJting 5:4dbed091ec5a 189 // // AnalogOut
JJting 5:4dbed091ec5a 190 // if (torque_measured*10 > 3.3) {
JJting 5:4dbed091ec5a 191 // torque_measured = 0.33;
JJting 5:4dbed091ec5a 192 // } else {
JJting 5:4dbed091ec5a 193 // test2 = torque_measured*10;
JJting 5:4dbed091ec5a 194 // }
JJting 5:4dbed091ec5a 195 // IMU
JJting 5:4dbed091ec5a 196 // imu.readAccel();
JJting 5:4dbed091ec5a 197 // imu.readGyro();
JJting 5:4dbed091ec5a 198 // imu2.readAccel();
JJting 5:4dbed091ec5a 199 // imu2.readGyro();
JJting 5:4dbed091ec5a 200 /*
JJting 5:4dbed091ec5a 201 Acce_axis_data[0] = imu.ax*Acce_gain_x;
JJting 5:4dbed091ec5a 202 Acce_axis_data[1] = imu.ay*Acce_gain_y;
JJting 5:4dbed091ec5a 203 Acce_axis_data[2] = imu.az*Acce_gain_z;
JJting 5:4dbed091ec5a 204 Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
JJting 5:4dbed091ec5a 205 Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
JJting 5:4dbed091ec5a 206 Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
JJting 5:4dbed091ec5a 207
JJting 5:4dbed091ec5a 208 Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
JJting 5:4dbed091ec5a 209 Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
JJting 5:4dbed091ec5a 210 Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
JJting 5:4dbed091ec5a 211 Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
JJting 5:4dbed091ec5a 212 Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
JJting 5:4dbed091ec5a 213 Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
JJting 5:4dbed091ec5a 214
JJting 5:4dbed091ec5a 215 for(i=0; i<6; i++) {
JJting 5:4dbed091ec5a 216 Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
JJting 5:4dbed091ec5a 217 Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
JJting 5:4dbed091ec5a 218 Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
JJting 5:4dbed091ec5a 219 Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
JJting 5:4dbed091ec5a 220 }
JJting 5:4dbed091ec5a 221
JJting 5:4dbed091ec5a 222 */
JJting 5:4dbed091ec5a 223 // dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 8:9c3b291b3288 224 // dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023
JJting 8:9c3b291b3288 225 dynamixelClass.wheel(SERVO_ID, 0, 300); //0~1023
JJting 7:5cb292622961 226 // wait_ms(1);
JJting 5:4dbed091ec5a 227 uart_tx();
JJting 5:4dbed091ec5a 228 flag = 0;
JJting 5:4dbed091ec5a 229 wait_ms(1);
JJting 5:4dbed091ec5a 230 test = 0;
JJting 0:c23e915f255b 231 }
JJting 0:c23e915f255b 232
JJting 0:c23e915f255b 233 void uart_tx()
JJting 0:c23e915f255b 234 {
JJting 0:c23e915f255b 235 splitter s1;
JJting 0:c23e915f255b 236 splitter s2;
JJting 0:c23e915f255b 237 splitter s3;
JJting 0:c23e915f255b 238 splitter s4;
JJting 0:c23e915f255b 239 splitter s5;
JJting 0:c23e915f255b 240 splitter s6;
JJting 0:c23e915f255b 241 splitter s7;
JJting 0:c23e915f255b 242
JJting 8:9c3b291b3288 243 s1.j = D_angle;
JJting 8:9c3b291b3288 244 // s2.j = Angle;
JJting 8:9c3b291b3288 245 // s3.j = Angle_Dif*360/4096;
JJting 8:9c3b291b3288 246 s2.j = D_Angle;
JJting 8:9c3b291b3288 247 s3.j = Angle;
JJting 7:5cb292622961 248 s4.j = _angle_difference*100;
JJting 7:5cb292622961 249 s5.j = torque_measured*100;
JJting 7:5cb292622961 250 s6.j = row_cmd;
JJting 1:2823a39f70a9 251 s7.j = 1;
JJting 0:c23e915f255b 252
JJting 0:c23e915f255b 253 T[2] = s1.C[0];
JJting 0:c23e915f255b 254 T[3] = s1.C[1];
JJting 0:c23e915f255b 255 T[4] = s2.C[0];
JJting 0:c23e915f255b 256 T[5] = s2.C[1];
JJting 0:c23e915f255b 257 T[6] = s3.C[0];
JJting 0:c23e915f255b 258 T[7] = s3.C[1];
JJting 0:c23e915f255b 259 T[8] = s4.C[0];
JJting 0:c23e915f255b 260 T[9] = s4.C[1];
JJting 0:c23e915f255b 261 T[10] = s5.C[0];
JJting 0:c23e915f255b 262 T[11] = s5.C[1];
JJting 0:c23e915f255b 263 T[12] = s6.C[0];
JJting 0:c23e915f255b 264 T[13] = s6.C[1];
JJting 0:c23e915f255b 265 T[14] = s7.C[0];
JJting 0:c23e915f255b 266 T[15] = s7.C[1];
JJting 0:c23e915f255b 267
JJting 0:c23e915f255b 268 while(1) {
JJting 0:c23e915f255b 269 if (uart.writeable() == 1) {
JJting 0:c23e915f255b 270 uart.putc(T[i]);
JJting 0:c23e915f255b 271 i++;
JJting 0:c23e915f255b 272 }
JJting 1:2823a39f70a9 273 if (i >= sizeof(T)) {
JJting 0:c23e915f255b 274 i = 0;
JJting 0:c23e915f255b 275 break;
JJting 0:c23e915f255b 276 }
JJting 0:c23e915f255b 277 }
JJting 0:c23e915f255b 278 }
JJting 0:c23e915f255b 279
JJting 0:c23e915f255b 280 void D_angle_measure()
JJting 0:c23e915f255b 281 {
JJting 0:c23e915f255b 282 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 283
JJting 0:c23e915f255b 284 if (d == 0) {
JJting 0:c23e915f255b 285 D_Angle = 0;
JJting 0:c23e915f255b 286 D_angle_old = D_angle;
JJting 0:c23e915f255b 287 d++;
JJting 0:c23e915f255b 288 } else {
JJting 7:5cb292622961 289
JJting 0:c23e915f255b 290 D_angle_dif = D_angle - D_angle_old;
JJting 7:5cb292622961 291
JJting 7:5cb292622961 292 if (D_angle_dif > 4096/2) {
JJting 7:5cb292622961 293 D_angle_dif = -(4096-(D_angle_dif));
JJting 7:5cb292622961 294 } else if (D_angle_dif < -4096/2) {
JJting 7:5cb292622961 295 D_angle_dif = -(-4096-(D_angle_dif));
JJting 7:5cb292622961 296 } else {
JJting 7:5cb292622961 297 D_angle_dif = D_angle_dif;
JJting 7:5cb292622961 298 }
JJting 7:5cb292622961 299
JJting 0:c23e915f255b 300 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 301 D_angle_old = D_angle;
JJting 7:5cb292622961 302
JJting 0:c23e915f255b 303 }
JJting 0:c23e915f255b 304 }
JJting 0:c23e915f255b 305
JJting 0:c23e915f255b 306 void find_torque()
JJting 0:c23e915f255b 307 {
JJting 1:2823a39f70a9 308
JJting 4:f19e7669d1b5 309 Angle_Dif = (-Angle*3+D_Angle);
JJting 4:f19e7669d1b5 310 // _angle_difference = (Angle*3-D_Angle)/4096.0f*2*_PI;
JJting 1:2823a39f70a9 311
JJting 4:f19e7669d1b5 312 _angle_difference = Angle_Dif/4096*2*_PI;
JJting 4:f19e7669d1b5 313 // if ((_angle_difference < 0) && (D_angle < 0)) {
JJting 6:f48c51662e27 314 // if (_angle_difference < -99) {
JJting 6:f48c51662e27 315 // _angle_difference = _angle_difference-100.54;
JJting 6:f48c51662e27 316 // }
JJting 6:f48c51662e27 317 // if (_angle_difference > 99) {
JJting 6:f48c51662e27 318 // _angle_difference = _angle_difference-100.54;
JJting 6:f48c51662e27 319 // }
JJting 7:5cb292622961 320 /* if (_angle_difference > 6) {
JJting 7:5cb292622961 321 _angle_difference = _angle_difference-6.4;
JJting 7:5cb292622961 322 }
JJting 7:5cb292622961 323 if (_angle_difference < -6) {
JJting 7:5cb292622961 324 _angle_difference = _angle_difference-6.4;
JJting 7:5cb292622961 325 }*/
JJting 4:f19e7669d1b5 326 torque_measured = _angle_difference*ks;
JJting 4:f19e7669d1b5 327 // torque_measured = Angle_Dif;
JJting 1:2823a39f70a9 328 }
JJting 3:98cdee5d9b63 329
JJting 3:98cdee5d9b63 330 float lpf(float input, float output_old, float frequency)
JJting 3:98cdee5d9b63 331 {
JJting 3:98cdee5d9b63 332 float output = 0;
JJting 3:98cdee5d9b63 333
JJting 3:98cdee5d9b63 334 output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
JJting 3:98cdee5d9b63 335
JJting 3:98cdee5d9b63 336 return output;
JJting 3:98cdee5d9b63 337 }