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@3:98cdee5d9b63, 2018-08-07 (annotated)

Committer:
JJting
Date:
Tue Aug 07 14:20:09 2018 +0000
Revision:
3:98cdee5d9b63
Parent:
2:f630aff31d27
Child:
4:f19e7669d1b5
ver1 with EMG read

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 3:98cdee5d9b63 22 AnalogIn EMG(PC_4);
JJting 0:c23e915f255b 23
JJting 0:c23e915f255b 24 // Timer
JJting 0:c23e915f255b 25 Ticker timer1;
JJting 1:2823a39f70a9 26 float ITR_time1 = 4000.0; // unit：us
JJting 0:c23e915f255b 27 float Ts = ITR_time1/1000000;
JJting 0:c23e915f255b 28 uint8_t flag;
JJting 0:c23e915f255b 29
JJting 3:98cdee5d9b63 30 // EMG
JJting 3:98cdee5d9b63 31 float lpf(float input, float output_old, float frequency);
JJting 3:98cdee5d9b63 32 float emg_value;
JJting 3:98cdee5d9b63 33 float emg_value_f;
JJting 3:98cdee5d9b63 34 float emg_value_f_old;
JJting 3:98cdee5d9b63 35 float Tf = ITR_time1*0.000001;
JJting 3:98cdee5d9b63 36
JJting 0:c23e915f255b 37 // uart_tx
JJting 0:c23e915f255b 38 union splitter {
JJting 0:c23e915f255b 39 short j;
JJting 0:c23e915f255b 40 char C[2];
JJting 0:c23e915f255b 41 // C[0] is lowbyte of j, C[1] is highbyte of j
JJting 0:c23e915f255b 42 };
JJting 0:c23e915f255b 43 char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
JJting 0:c23e915f255b 44 int i = 0;
JJting 0:c23e915f255b 45
JJting 0:c23e915f255b 46 // PID
JJting 0:c23e915f255b 47 PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13
JJting 0:c23e915f255b 48 float PIDout = 0.0f;
JJting 0:c23e915f255b 49
JJting 0:c23e915f255b 50 // Dynamixel
JJting 0:c23e915f255b 51 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
JJting 0:c23e915f255b 52 int servo_cmd;
JJting 1:2823a39f70a9 53 int row_cmd;
JJting 0:c23e915f255b 54 int D_angle = 0;
JJting 0:c23e915f255b 55 int D_angle_dif = 0;
JJting 0:c23e915f255b 56 int D_Angle;
JJting 0:c23e915f255b 57 int D_angle_old;
JJting 0:c23e915f255b 58 unsigned short d = 0;
JJting 0:c23e915f255b 59 // Find Torque
JJting 0:c23e915f255b 60 double angle_difference = 0.0;
JJting 0:c23e915f255b 61 float torque_measured = 0.0;
JJting 0:c23e915f255b 62 float ks = 2.6393*2; //spring constant
JJting 0:c23e915f255b 63 //int angle_dif = 0;
JJting 0:c23e915f255b 64 float torque_ref = 0.0;
JJting 1:2823a39f70a9 65 //float friction = 0.0f;
JJting 0:c23e915f255b 66 float friction = 0.18f;
JJting 1:2823a39f70a9 67 float rate = 0.5;
JJting 0:c23e915f255b 68 //float friction = 0.0f;
JJting 0:c23e915f255b 69 //float check = 0.0f;
JJting 1:2823a39f70a9 70 float Angle_Dif;
JJting 0:c23e915f255b 71
JJting 0:c23e915f255b 72 // function
JJting 0:c23e915f255b 73 void init_UART();
JJting 0:c23e915f255b 74 void init_TIMER();
JJting 0:c23e915f255b 75 void timer1_ITR();
JJting 0:c23e915f255b 76 void uart_tx();
JJting 0:c23e915f255b 77
JJting 0:c23e915f255b 78 void init_DYNAMIXEL();
JJting 0:c23e915f255b 79 void D_angle_measure();
JJting 0:c23e915f255b 80 void find_torque();
JJting 0:c23e915f255b 81
JJting 0:c23e915f255b 82 int main()
JJting 0:c23e915f255b 83 {
JJting 0:c23e915f255b 84 LED = 1; // darken
JJting 0:c23e915f255b 85 wait_ms(500);
JJting 0:c23e915f255b 86 // initial sensor
JJting 0:c23e915f255b 87 init_SPI_encoder();
JJting 0:c23e915f255b 88 init_encoder();
JJting 0:c23e915f255b 89 init_DYNAMIXEL();
JJting 0:c23e915f255b 90 // initial uart
JJting 0:c23e915f255b 91 init_UART();
JJting 0:c23e915f255b 92
JJting 0:c23e915f255b 93 wait_ms(500);
JJting 0:c23e915f255b 94
JJting 0:c23e915f255b 95 led2 = 1;
JJting 1:2823a39f70a9 96 // led2f = 0;
JJting 0:c23e915f255b 97 LED = 0; // lighten
JJting 0:c23e915f255b 98
JJting 0:c23e915f255b 99 init_TIMER();
JJting 0:c23e915f255b 100
JJting 0:c23e915f255b 101 while(1) {
JJting 0:c23e915f255b 102 if (flag==1) {
JJting 0:c23e915f255b 103 led2 = !led2;
JJting 0:c23e915f255b 104 angle_measure();
JJting 0:c23e915f255b 105 D_angle_measure();
JJting 0:c23e915f255b 106 find_torque();
JJting 0:c23e915f255b 107 motor_pid.Compute(torque_ref, torque_measured);
JJting 0:c23e915f255b 108 PIDout = motor_pid.output;
JJting 1:2823a39f70a9 109 servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857
JJting 0:c23e915f255b 110
JJting 0:c23e915f255b 111 if (servo_cmd > 0) {
JJting 0:c23e915f255b 112 servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 113 if (servo_cmd >= 1023)
JJting 0:c23e915f255b 114 servo_cmd = 1023;
JJting 0:c23e915f255b 115 } else {
JJting 0:c23e915f255b 116 servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 117 if (servo_cmd >= 2047)
JJting 0:c23e915f255b 118 servo_cmd = 2047;
JJting 0:c23e915f255b 119 }
JJting 1:2823a39f70a9 120
JJting 1:2823a39f70a9 121 if (servo_cmd >= 1023) {
JJting 1:2823a39f70a9 122 row_cmd = -(servo_cmd-1023);
JJting 1:2823a39f70a9 123 } else {
JJting 1:2823a39f70a9 124 row_cmd = servo_cmd;
JJting 1:2823a39f70a9 125 }
JJting 1:2823a39f70a9 126
JJting 3:98cdee5d9b63 127 // EMG
JJting 3:98cdee5d9b63 128 emg_value = EMG.read();
JJting 3:98cdee5d9b63 129 emg_value_f = lpf(emg_value,emg_value_f_old,15);
JJting 3:98cdee5d9b63 130 emg_value_f_old = emg_value_f;
JJting 3:98cdee5d9b63 131 // dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 0:c23e915f255b 132 uart_tx();
JJting 0:c23e915f255b 133 flag = 0;
JJting 0:c23e915f255b 134 }
JJting 0:c23e915f255b 135 }
JJting 0:c23e915f255b 136 }
JJting 0:c23e915f255b 137
JJting 0:c23e915f255b 138 void init_DYNAMIXEL()
JJting 0:c23e915f255b 139 {
JJting 0:c23e915f255b 140 dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 0:c23e915f255b 141 wait_ms(1);
JJting 0:c23e915f255b 142 }
JJting 0:c23e915f255b 143
JJting 0:c23e915f255b 144 void init_UART()
JJting 0:c23e915f255b 145 {
JJting 0:c23e915f255b 146 uart.baud(115200);
JJting 0:c23e915f255b 147 }
JJting 0:c23e915f255b 148
JJting 0:c23e915f255b 149 void init_TIMER()
JJting 0:c23e915f255b 150 {
JJting 0:c23e915f255b 151 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 152 }
JJting 0:c23e915f255b 153
JJting 0:c23e915f255b 154 void timer1_ITR()
JJting 0:c23e915f255b 155 {
JJting 0:c23e915f255b 156 flag = 1;
JJting 1:2823a39f70a9 157 // if (led2f == 5) {
JJting 1:2823a39f70a9 158 //
JJting 1:2823a39f70a9 159 // led2f = 0;
JJting 1:2823a39f70a9 160 // } else {
JJting 1:2823a39f70a9 161 // led2f++;
JJting 1:2823a39f70a9 162 // }
JJting 0:c23e915f255b 163 }
JJting 0:c23e915f255b 164
JJting 0:c23e915f255b 165 void uart_tx()
JJting 0:c23e915f255b 166 {
JJting 0:c23e915f255b 167 splitter s1;
JJting 0:c23e915f255b 168 splitter s2;
JJting 0:c23e915f255b 169 splitter s3;
JJting 0:c23e915f255b 170 splitter s4;
JJting 0:c23e915f255b 171 splitter s5;
JJting 0:c23e915f255b 172 splitter s6;
JJting 0:c23e915f255b 173 splitter s7;
JJting 0:c23e915f255b 174
JJting 1:2823a39f70a9 175 s1.j = 1;
JJting 0:c23e915f255b 176 s2.j = Angle;
JJting 1:2823a39f70a9 177 s3.j = D_Angle;
JJting 1:2823a39f70a9 178 // s3.j = servo_cmd;
JJting 0:c23e915f255b 179 // s4.j = 1;
JJting 0:c23e915f255b 180 // s5.j = 3;
JJting 2:f630aff31d27 181 s4.j = (Angle*3-D_Angle)*360/4096;
JJting 3:98cdee5d9b63 182 s5.j = emg_value_f*100;
JJting 1:2823a39f70a9 183 s6.j = row_cmd;
JJting 1:2823a39f70a9 184 s7.j = 1;
JJting 0:c23e915f255b 185
JJting 0:c23e915f255b 186 T[2] = s1.C[0];
JJting 0:c23e915f255b 187 T[3] = s1.C[1];
JJting 0:c23e915f255b 188 T[4] = s2.C[0];
JJting 0:c23e915f255b 189 T[5] = s2.C[1];
JJting 0:c23e915f255b 190 T[6] = s3.C[0];
JJting 0:c23e915f255b 191 T[7] = s3.C[1];
JJting 0:c23e915f255b 192 T[8] = s4.C[0];
JJting 0:c23e915f255b 193 T[9] = s4.C[1];
JJting 0:c23e915f255b 194 T[10] = s5.C[0];
JJting 0:c23e915f255b 195 T[11] = s5.C[1];
JJting 0:c23e915f255b 196 T[12] = s6.C[0];
JJting 0:c23e915f255b 197 T[13] = s6.C[1];
JJting 0:c23e915f255b 198 T[14] = s7.C[0];
JJting 0:c23e915f255b 199 T[15] = s7.C[1];
JJting 0:c23e915f255b 200
JJting 0:c23e915f255b 201 while(1) {
JJting 0:c23e915f255b 202 if (uart.writeable() == 1) {
JJting 0:c23e915f255b 203 uart.putc(T[i]);
JJting 0:c23e915f255b 204 i++;
JJting 0:c23e915f255b 205 }
JJting 1:2823a39f70a9 206 if (i >= sizeof(T)) {
JJting 0:c23e915f255b 207 i = 0;
JJting 0:c23e915f255b 208 break;
JJting 0:c23e915f255b 209 }
JJting 0:c23e915f255b 210 }
JJting 0:c23e915f255b 211 }
JJting 0:c23e915f255b 212
JJting 0:c23e915f255b 213 void D_angle_measure()
JJting 0:c23e915f255b 214 {
JJting 0:c23e915f255b 215 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 216
JJting 0:c23e915f255b 217 if (d == 0) {
JJting 0:c23e915f255b 218 D_Angle = 0;
JJting 0:c23e915f255b 219 D_angle_old = D_angle;
JJting 0:c23e915f255b 220 d++;
JJting 0:c23e915f255b 221 } else {
JJting 0:c23e915f255b 222 D_angle_dif = D_angle - D_angle_old;
JJting 0:c23e915f255b 223 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 224 D_angle_old = D_angle;
JJting 0:c23e915f255b 225 }
JJting 0:c23e915f255b 226 }
JJting 0:c23e915f255b 227
JJting 0:c23e915f255b 228 void find_torque()
JJting 0:c23e915f255b 229 {
JJting 1:2823a39f70a9 230
JJting 1:2823a39f70a9 231 // Angle_Dif = Angle*3-D_Angle;
JJting 0:c23e915f255b 232 angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI;
JJting 1:2823a39f70a9 233 // angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 234
JJting 0:c23e915f255b 235 torque_measured = angle_difference*ks;
JJting 1:2823a39f70a9 236 }
JJting 3:98cdee5d9b63 237
JJting 3:98cdee5d9b63 238 float lpf(float input, float output_old, float frequency)
JJting 3:98cdee5d9b63 239 {
JJting 3:98cdee5d9b63 240 float output = 0;
JJting 3:98cdee5d9b63 241
JJting 3:98cdee5d9b63 242 output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
JJting 3:98cdee5d9b63 243
JJting 3:98cdee5d9b63 244 return output;
JJting 3:98cdee5d9b63 245 }