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@2:f630aff31d27, 2018-08-07 (annotated)

Committer:
JJting
Date:
Tue Aug 07 12:01:03 2018 +0000
Revision:
2:f630aff31d27
Parent:
1:2823a39f70a9
Child:
3:98cdee5d9b63
ver3 still some problem with 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 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 0:c23e915f255b 39 PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13
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 0:c23e915f255b 63
JJting 0:c23e915f255b 64 // function
JJting 0:c23e915f255b 65 void init_UART();
JJting 0:c23e915f255b 66 void init_TIMER();
JJting 0:c23e915f255b 67 void timer1_ITR();
JJting 0:c23e915f255b 68 void uart_tx();
JJting 0:c23e915f255b 69
JJting 0:c23e915f255b 70 void init_DYNAMIXEL();
JJting 0:c23e915f255b 71 void D_angle_measure();
JJting 0:c23e915f255b 72 void find_torque();
JJting 0:c23e915f255b 73
JJting 0:c23e915f255b 74 int main()
JJting 0:c23e915f255b 75 {
JJting 0:c23e915f255b 76 LED = 1; // darken
JJting 0:c23e915f255b 77 wait_ms(500);
JJting 0:c23e915f255b 78 // initial sensor
JJting 0:c23e915f255b 79 init_SPI_encoder();
JJting 0:c23e915f255b 80 init_encoder();
JJting 0:c23e915f255b 81 init_DYNAMIXEL();
JJting 0:c23e915f255b 82 // initial uart
JJting 0:c23e915f255b 83 init_UART();
JJting 0:c23e915f255b 84
JJting 0:c23e915f255b 85 wait_ms(500);
JJting 0:c23e915f255b 86
JJting 0:c23e915f255b 87 led2 = 1;
JJting 1:2823a39f70a9 88 // led2f = 0;
JJting 0:c23e915f255b 89 LED = 0; // lighten
JJting 0:c23e915f255b 90
JJting 0:c23e915f255b 91 init_TIMER();
JJting 0:c23e915f255b 92
JJting 0:c23e915f255b 93 while(1) {
JJting 0:c23e915f255b 94 if (flag==1) {
JJting 0:c23e915f255b 95 led2 = !led2;
JJting 0:c23e915f255b 96 angle_measure();
JJting 0:c23e915f255b 97 D_angle_measure();
JJting 0:c23e915f255b 98 find_torque();
JJting 0:c23e915f255b 99 motor_pid.Compute(torque_ref, torque_measured);
JJting 0:c23e915f255b 100 PIDout = motor_pid.output;
JJting 1:2823a39f70a9 101 servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857
JJting 0:c23e915f255b 102
JJting 0:c23e915f255b 103 if (servo_cmd > 0) {
JJting 0:c23e915f255b 104 servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 105 if (servo_cmd >= 1023)
JJting 0:c23e915f255b 106 servo_cmd = 1023;
JJting 0:c23e915f255b 107 } else {
JJting 0:c23e915f255b 108 servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 109 if (servo_cmd >= 2047)
JJting 0:c23e915f255b 110 servo_cmd = 2047;
JJting 0:c23e915f255b 111 }
JJting 1:2823a39f70a9 112
JJting 1:2823a39f70a9 113 if (servo_cmd >= 1023) {
JJting 1:2823a39f70a9 114 row_cmd = -(servo_cmd-1023);
JJting 1:2823a39f70a9 115 } else {
JJting 1:2823a39f70a9 116 row_cmd = servo_cmd;
JJting 1:2823a39f70a9 117 }
JJting 1:2823a39f70a9 118
JJting 2:f630aff31d27 119 dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 0:c23e915f255b 120 uart_tx();
JJting 0:c23e915f255b 121 flag = 0;
JJting 0:c23e915f255b 122 }
JJting 0:c23e915f255b 123 }
JJting 0:c23e915f255b 124 }
JJting 0:c23e915f255b 125
JJting 0:c23e915f255b 126 void init_DYNAMIXEL()
JJting 0:c23e915f255b 127 {
JJting 0:c23e915f255b 128 dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 0:c23e915f255b 129 wait_ms(1);
JJting 0:c23e915f255b 130 }
JJting 0:c23e915f255b 131
JJting 0:c23e915f255b 132 void init_UART()
JJting 0:c23e915f255b 133 {
JJting 0:c23e915f255b 134 uart.baud(115200);
JJting 0:c23e915f255b 135 }
JJting 0:c23e915f255b 136
JJting 0:c23e915f255b 137 void init_TIMER()
JJting 0:c23e915f255b 138 {
JJting 0:c23e915f255b 139 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 140 }
JJting 0:c23e915f255b 141
JJting 0:c23e915f255b 142 void timer1_ITR()
JJting 0:c23e915f255b 143 {
JJting 0:c23e915f255b 144 flag = 1;
JJting 1:2823a39f70a9 145 // if (led2f == 5) {
JJting 1:2823a39f70a9 146 //
JJting 1:2823a39f70a9 147 // led2f = 0;
JJting 1:2823a39f70a9 148 // } else {
JJting 1:2823a39f70a9 149 // led2f++;
JJting 1:2823a39f70a9 150 // }
JJting 0:c23e915f255b 151 }
JJting 0:c23e915f255b 152
JJting 0:c23e915f255b 153 void uart_tx()
JJting 0:c23e915f255b 154 {
JJting 0:c23e915f255b 155 splitter s1;
JJting 0:c23e915f255b 156 splitter s2;
JJting 0:c23e915f255b 157 splitter s3;
JJting 0:c23e915f255b 158 splitter s4;
JJting 0:c23e915f255b 159 splitter s5;
JJting 0:c23e915f255b 160 splitter s6;
JJting 0:c23e915f255b 161 splitter s7;
JJting 0:c23e915f255b 162
JJting 1:2823a39f70a9 163 s1.j = 1;
JJting 0:c23e915f255b 164 s2.j = Angle;
JJting 1:2823a39f70a9 165 s3.j = D_Angle;
JJting 1:2823a39f70a9 166 // s3.j = servo_cmd;
JJting 0:c23e915f255b 167 // s4.j = 1;
JJting 0:c23e915f255b 168 // s5.j = 3;
JJting 2:f630aff31d27 169 s4.j = (Angle*3-D_Angle)*360/4096;
JJting 1:2823a39f70a9 170 s5.j = torque_measured*1000;
JJting 1:2823a39f70a9 171 s6.j = row_cmd;
JJting 1:2823a39f70a9 172 s7.j = 1;
JJting 0:c23e915f255b 173
JJting 0:c23e915f255b 174 T[2] = s1.C[0];
JJting 0:c23e915f255b 175 T[3] = s1.C[1];
JJting 0:c23e915f255b 176 T[4] = s2.C[0];
JJting 0:c23e915f255b 177 T[5] = s2.C[1];
JJting 0:c23e915f255b 178 T[6] = s3.C[0];
JJting 0:c23e915f255b 179 T[7] = s3.C[1];
JJting 0:c23e915f255b 180 T[8] = s4.C[0];
JJting 0:c23e915f255b 181 T[9] = s4.C[1];
JJting 0:c23e915f255b 182 T[10] = s5.C[0];
JJting 0:c23e915f255b 183 T[11] = s5.C[1];
JJting 0:c23e915f255b 184 T[12] = s6.C[0];
JJting 0:c23e915f255b 185 T[13] = s6.C[1];
JJting 0:c23e915f255b 186 T[14] = s7.C[0];
JJting 0:c23e915f255b 187 T[15] = s7.C[1];
JJting 0:c23e915f255b 188
JJting 0:c23e915f255b 189 while(1) {
JJting 0:c23e915f255b 190 if (uart.writeable() == 1) {
JJting 0:c23e915f255b 191 uart.putc(T[i]);
JJting 0:c23e915f255b 192 i++;
JJting 0:c23e915f255b 193 }
JJting 1:2823a39f70a9 194 if (i >= sizeof(T)) {
JJting 0:c23e915f255b 195 i = 0;
JJting 0:c23e915f255b 196 break;
JJting 0:c23e915f255b 197 }
JJting 0:c23e915f255b 198 }
JJting 0:c23e915f255b 199 }
JJting 0:c23e915f255b 200
JJting 0:c23e915f255b 201 void D_angle_measure()
JJting 0:c23e915f255b 202 {
JJting 0:c23e915f255b 203 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 204
JJting 0:c23e915f255b 205 if (d == 0) {
JJting 0:c23e915f255b 206 D_Angle = 0;
JJting 0:c23e915f255b 207 D_angle_old = D_angle;
JJting 0:c23e915f255b 208 d++;
JJting 0:c23e915f255b 209 } else {
JJting 0:c23e915f255b 210 D_angle_dif = D_angle - D_angle_old;
JJting 0:c23e915f255b 211 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 212 D_angle_old = D_angle;
JJting 0:c23e915f255b 213 }
JJting 0:c23e915f255b 214 }
JJting 0:c23e915f255b 215
JJting 0:c23e915f255b 216 void find_torque()
JJting 0:c23e915f255b 217 {
JJting 1:2823a39f70a9 218
JJting 1:2823a39f70a9 219 // Angle_Dif = Angle*3-D_Angle;
JJting 0:c23e915f255b 220 angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI;
JJting 1:2823a39f70a9 221 // angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 222
JJting 0:c23e915f255b 223 torque_measured = angle_difference*ks;
JJting 1:2823a39f70a9 224 }