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@3:c05acc05c3bd, 2018-08-24 (annotated)

Committer:
JJting
Date:
Fri Aug 24 02:47:02 2018 +0000
Revision:
3:c05acc05c3bd
Parent:
2:f630aff31d27
Child:
4:a59512fe0f9a
???????????

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 3:c05acc05c3bd 39 PID motor_pid(10, 0, 0, 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
JJting 0:c23e915f255b 95 }
JJting 0:c23e915f255b 96 }
JJting 0:c23e915f255b 97
JJting 0:c23e915f255b 98 void init_DYNAMIXEL()
JJting 0:c23e915f255b 99 {
JJting 0:c23e915f255b 100 dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 3:c05acc05c3bd 101 // dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
JJting 0:c23e915f255b 102 wait_ms(1);
JJting 0:c23e915f255b 103 }
JJting 0:c23e915f255b 104
JJting 0:c23e915f255b 105 void init_UART()
JJting 0:c23e915f255b 106 {
JJting 0:c23e915f255b 107 uart.baud(115200);
JJting 0:c23e915f255b 108 }
JJting 0:c23e915f255b 109
JJting 0:c23e915f255b 110 void init_TIMER()
JJting 0:c23e915f255b 111 {
JJting 0:c23e915f255b 112 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 113 }
JJting 0:c23e915f255b 114
JJting 0:c23e915f255b 115 void timer1_ITR()
JJting 0:c23e915f255b 116 {
JJting 3:c05acc05c3bd 117 led2 = !led2;
JJting 3:c05acc05c3bd 118 angle_measure();
JJting 3:c05acc05c3bd 119 D_angle_measure();
JJting 3:c05acc05c3bd 120 find_torque();
JJting 3:c05acc05c3bd 121 motor_pid.Compute(torque_ref, torque_measured);
JJting 3:c05acc05c3bd 122 PIDout = motor_pid.output;
JJting 3:c05acc05c3bd 123 servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857
JJting 3:c05acc05c3bd 124
JJting 3:c05acc05c3bd 125 if (servo_cmd > 0) {
JJting 3:c05acc05c3bd 126 // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
JJting 3:c05acc05c3bd 127 servo_cmd = servo_cmd;
JJting 3:c05acc05c3bd 128 if (servo_cmd >= 1023)
JJting 3:c05acc05c3bd 129 servo_cmd = 1023;
JJting 3:c05acc05c3bd 130 } else {
JJting 3:c05acc05c3bd 131 // servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
JJting 3:c05acc05c3bd 132 servo_cmd = -servo_cmd + 1024;
JJting 3:c05acc05c3bd 133 if (servo_cmd >= 2047)
JJting 3:c05acc05c3bd 134 servo_cmd = 2047;
JJting 3:c05acc05c3bd 135 }
JJting 3:c05acc05c3bd 136
JJting 3:c05acc05c3bd 137 if (servo_cmd >= 1023) {
JJting 3:c05acc05c3bd 138 row_cmd = -(servo_cmd-1023);
JJting 3:c05acc05c3bd 139 } else {
JJting 3:c05acc05c3bd 140 row_cmd = servo_cmd;
JJting 3:c05acc05c3bd 141 }
JJting 3:c05acc05c3bd 142
JJting 3:c05acc05c3bd 143 dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 3:c05acc05c3bd 144 // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation)
JJting 3:c05acc05c3bd 145 uart_tx();
JJting 0:c23e915f255b 146 }
JJting 0:c23e915f255b 147
JJting 0:c23e915f255b 148 void uart_tx()
JJting 0:c23e915f255b 149 {
JJting 0:c23e915f255b 150 splitter s1;
JJting 0:c23e915f255b 151 splitter s2;
JJting 0:c23e915f255b 152 splitter s3;
JJting 0:c23e915f255b 153 splitter s4;
JJting 0:c23e915f255b 154 splitter s5;
JJting 0:c23e915f255b 155 splitter s6;
JJting 0:c23e915f255b 156 splitter s7;
JJting 0:c23e915f255b 157
JJting 3:c05acc05c3bd 158 s1.j = D_Angle;
JJting 3:c05acc05c3bd 159 s2.j = Angle*3;
JJting 3:c05acc05c3bd 160 s3.j = Angle_Dif*360/4096;
JJting 1:2823a39f70a9 161 // s3.j = servo_cmd;
JJting 0:c23e915f255b 162 // s4.j = 1;
JJting 0:c23e915f255b 163 // s5.j = 3;
JJting 3:c05acc05c3bd 164 s4.j = D_angle;
JJting 1:2823a39f70a9 165 s5.j = torque_measured*1000;
JJting 1:2823a39f70a9 166 s6.j = row_cmd;
JJting 1:2823a39f70a9 167 s7.j = 1;
JJting 0:c23e915f255b 168
JJting 0:c23e915f255b 169 T[2] = s1.C[0];
JJting 0:c23e915f255b 170 T[3] = s1.C[1];
JJting 0:c23e915f255b 171 T[4] = s2.C[0];
JJting 0:c23e915f255b 172 T[5] = s2.C[1];
JJting 0:c23e915f255b 173 T[6] = s3.C[0];
JJting 0:c23e915f255b 174 T[7] = s3.C[1];
JJting 0:c23e915f255b 175 T[8] = s4.C[0];
JJting 0:c23e915f255b 176 T[9] = s4.C[1];
JJting 0:c23e915f255b 177 T[10] = s5.C[0];
JJting 0:c23e915f255b 178 T[11] = s5.C[1];
JJting 0:c23e915f255b 179 T[12] = s6.C[0];
JJting 0:c23e915f255b 180 T[13] = s6.C[1];
JJting 0:c23e915f255b 181 T[14] = s7.C[0];
JJting 0:c23e915f255b 182 T[15] = s7.C[1];
JJting 0:c23e915f255b 183
JJting 0:c23e915f255b 184 while(1) {
JJting 0:c23e915f255b 185 if (uart.writeable() == 1) {
JJting 0:c23e915f255b 186 uart.putc(T[i]);
JJting 0:c23e915f255b 187 i++;
JJting 0:c23e915f255b 188 }
JJting 1:2823a39f70a9 189 if (i >= sizeof(T)) {
JJting 0:c23e915f255b 190 i = 0;
JJting 0:c23e915f255b 191 break;
JJting 0:c23e915f255b 192 }
JJting 0:c23e915f255b 193 }
JJting 0:c23e915f255b 194 }
JJting 0:c23e915f255b 195
JJting 0:c23e915f255b 196 void D_angle_measure()
JJting 0:c23e915f255b 197 {
JJting 0:c23e915f255b 198 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 199
JJting 0:c23e915f255b 200 if (d == 0) {
JJting 0:c23e915f255b 201 D_Angle = 0;
JJting 0:c23e915f255b 202 D_angle_old = D_angle;
JJting 0:c23e915f255b 203 d++;
JJting 0:c23e915f255b 204 } else {
JJting 0:c23e915f255b 205 D_angle_dif = D_angle - D_angle_old;
JJting 3:c05acc05c3bd 206
JJting 3:c05acc05c3bd 207 if (D_angle_dif > 4096/2) {
JJting 3:c05acc05c3bd 208 D_angle_dif = -(4096-(D_angle_dif));
JJting 3:c05acc05c3bd 209 } else if (D_angle_dif < -4096/2) {
JJting 3:c05acc05c3bd 210 D_angle_dif = -(-4096-(D_angle_dif));
JJting 3:c05acc05c3bd 211 } else {
JJting 3:c05acc05c3bd 212 D_angle_dif = D_angle_dif;
JJting 3:c05acc05c3bd 213 }
JJting 3:c05acc05c3bd 214
JJting 0:c23e915f255b 215 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 216 D_angle_old = D_angle;
JJting 0:c23e915f255b 217 }
JJting 0:c23e915f255b 218 }
JJting 0:c23e915f255b 219
JJting 0:c23e915f255b 220 void find_torque()
JJting 0:c23e915f255b 221 {
JJting 1:2823a39f70a9 222
JJting 3:c05acc05c3bd 223 Angle_Dif = Angle*3-D_Angle;
JJting 3:c05acc05c3bd 224 angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 225 // angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 226
JJting 0:c23e915f255b 227 torque_measured = angle_difference*ks;
JJting 1:2823a39f70a9 228 }