SHENG-HEN HSIEH / Mbed 2 deprecated LSM9DS1_project_5_zerotorque

By pOOPOO

Dependencies:   MX28 mbed

Fork of LSM9DS1_project_5_zerotorque by Rong Syuan Lin

main.cpp@0:c23e915f255b, 2018-08-05 (annotated)

Committer:
JJting
Date:
Sun Aug 05 13:15:56 2018 +0000
Revision:
0:c23e915f255b
Child:
1:2823a39f70a9
ver1

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 0:c23e915f255b 25 float ITR_time1 = 10000.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 0:c23e915f255b 45 int D_angle = 0;
JJting 0:c23e915f255b 46 int D_angle_dif = 0;
JJting 0:c23e915f255b 47 int D_Angle;
JJting 0:c23e915f255b 48 int D_angle_old;
JJting 0:c23e915f255b 49 unsigned short d = 0;
JJting 0:c23e915f255b 50 // Find Torque
JJting 0:c23e915f255b 51 double angle_difference = 0.0;
JJting 0:c23e915f255b 52 float torque_measured = 0.0;
JJting 0:c23e915f255b 53 float ks = 2.6393*2; //spring constant
JJting 0:c23e915f255b 54 //int angle_dif = 0;
JJting 0:c23e915f255b 55 float torque_ref = 0.0;
JJting 0:c23e915f255b 56 float friction = 0.18f;
JJting 0:c23e915f255b 57 float rate = 0.8;
JJting 0:c23e915f255b 58 //float friction = 0.0f;
JJting 0:c23e915f255b 59 //float check = 0.0f;
JJting 0:c23e915f255b 60
JJting 0:c23e915f255b 61 // function
JJting 0:c23e915f255b 62 void init_UART();
JJting 0:c23e915f255b 63 void init_TIMER();
JJting 0:c23e915f255b 64 void timer1_ITR();
JJting 0:c23e915f255b 65 void uart_tx();
JJting 0:c23e915f255b 66
JJting 0:c23e915f255b 67 void init_DYNAMIXEL();
JJting 0:c23e915f255b 68 void D_angle_measure();
JJting 0:c23e915f255b 69 void find_torque();
JJting 0:c23e915f255b 70
JJting 0:c23e915f255b 71 int main()
JJting 0:c23e915f255b 72 {
JJting 0:c23e915f255b 73 LED = 1; // darken
JJting 0:c23e915f255b 74 wait_ms(500);
JJting 0:c23e915f255b 75 // initial sensor
JJting 0:c23e915f255b 76 init_SPI_encoder();
JJting 0:c23e915f255b 77 init_encoder();
JJting 0:c23e915f255b 78 init_DYNAMIXEL();
JJting 0:c23e915f255b 79 // initial uart
JJting 0:c23e915f255b 80 init_UART();
JJting 0:c23e915f255b 81
JJting 0:c23e915f255b 82 wait_ms(500);
JJting 0:c23e915f255b 83
JJting 0:c23e915f255b 84 led2 = 1;
JJting 0:c23e915f255b 85 led2f = 0;
JJting 0:c23e915f255b 86 LED = 0; // lighten
JJting 0:c23e915f255b 87
JJting 0:c23e915f255b 88 init_TIMER();
JJting 0:c23e915f255b 89
JJting 0:c23e915f255b 90 while(1) {
JJting 0:c23e915f255b 91 if (flag==1) {
JJting 0:c23e915f255b 92 led2 = !led2;
JJting 0:c23e915f255b 93 angle_measure();
JJting 0:c23e915f255b 94 D_angle_measure();
JJting 0:c23e915f255b 95 find_torque();
JJting 0:c23e915f255b 96 motor_pid.Compute(torque_ref, torque_measured);
JJting 0:c23e915f255b 97 PIDout = motor_pid.output;
JJting 0:c23e915f255b 98 servo_cmd = PIDout*121.8f; // 1023/8.4Nm = 121.7857
JJting 0:c23e915f255b 99
JJting 0:c23e915f255b 100 if (servo_cmd > 0) {
JJting 0:c23e915f255b 101 servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 102 if (servo_cmd >= 1023)
JJting 0:c23e915f255b 103 servo_cmd = 1023;
JJting 0:c23e915f255b 104 } else {
JJting 0:c23e915f255b 105 servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f;
JJting 0:c23e915f255b 106 if (servo_cmd >= 2047)
JJting 0:c23e915f255b 107 servo_cmd = 2047;
JJting 0:c23e915f255b 108 }
JJting 0:c23e915f255b 109 // dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 0:c23e915f255b 110 uart_tx();
JJting 0:c23e915f255b 111 flag = 0;
JJting 0:c23e915f255b 112 }
JJting 0:c23e915f255b 113 }
JJting 0:c23e915f255b 114 }
JJting 0:c23e915f255b 115
JJting 0:c23e915f255b 116 void init_DYNAMIXEL()
JJting 0:c23e915f255b 117 {
JJting 0:c23e915f255b 118 dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 0:c23e915f255b 119 wait_ms(1);
JJting 0:c23e915f255b 120 }
JJting 0:c23e915f255b 121
JJting 0:c23e915f255b 122 void init_UART()
JJting 0:c23e915f255b 123 {
JJting 0:c23e915f255b 124 uart.baud(115200);
JJting 0:c23e915f255b 125 }
JJting 0:c23e915f255b 126
JJting 0:c23e915f255b 127 void init_TIMER()
JJting 0:c23e915f255b 128 {
JJting 0:c23e915f255b 129 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 130 }
JJting 0:c23e915f255b 131
JJting 0:c23e915f255b 132 void timer1_ITR()
JJting 0:c23e915f255b 133 {
JJting 0:c23e915f255b 134 flag = 1;
JJting 0:c23e915f255b 135 if (led2f == 5) {
JJting 0:c23e915f255b 136
JJting 0:c23e915f255b 137 led2f = 0;
JJting 0:c23e915f255b 138 } else {
JJting 0:c23e915f255b 139 led2f++;
JJting 0:c23e915f255b 140 }
JJting 0:c23e915f255b 141 }
JJting 0:c23e915f255b 142
JJting 0:c23e915f255b 143 void uart_tx()
JJting 0:c23e915f255b 144 {
JJting 0:c23e915f255b 145 splitter s1;
JJting 0:c23e915f255b 146 splitter s2;
JJting 0:c23e915f255b 147 splitter s3;
JJting 0:c23e915f255b 148 splitter s4;
JJting 0:c23e915f255b 149 splitter s5;
JJting 0:c23e915f255b 150 splitter s6;
JJting 0:c23e915f255b 151 splitter s7;
JJting 0:c23e915f255b 152
JJting 0:c23e915f255b 153 s1.j = angle;
JJting 0:c23e915f255b 154 s2.j = Angle;
JJting 0:c23e915f255b 155 s3.j = 2;
JJting 0:c23e915f255b 156 // s4.j = 1;
JJting 0:c23e915f255b 157 // s5.j = 3;
JJting 0:c23e915f255b 158 s4.j = D_angle;
JJting 0:c23e915f255b 159 s5.j = D_Angle;
JJting 0:c23e915f255b 160 s6.j = torque_measured*1000;
JJting 0:c23e915f255b 161 s7.j = 3;
JJting 0:c23e915f255b 162
JJting 0:c23e915f255b 163 T[2] = s1.C[0];
JJting 0:c23e915f255b 164 T[3] = s1.C[1];
JJting 0:c23e915f255b 165 T[4] = s2.C[0];
JJting 0:c23e915f255b 166 T[5] = s2.C[1];
JJting 0:c23e915f255b 167 T[6] = s3.C[0];
JJting 0:c23e915f255b 168 T[7] = s3.C[1];
JJting 0:c23e915f255b 169 T[8] = s4.C[0];
JJting 0:c23e915f255b 170 T[9] = s4.C[1];
JJting 0:c23e915f255b 171 T[10] = s5.C[0];
JJting 0:c23e915f255b 172 T[11] = s5.C[1];
JJting 0:c23e915f255b 173 T[12] = s6.C[0];
JJting 0:c23e915f255b 174 T[13] = s6.C[1];
JJting 0:c23e915f255b 175 T[14] = s7.C[0];
JJting 0:c23e915f255b 176 T[15] = s7.C[1];
JJting 0:c23e915f255b 177
JJting 0:c23e915f255b 178 while(1) {
JJting 0:c23e915f255b 179 if (uart.writeable() == 1) {
JJting 0:c23e915f255b 180 uart.putc(T[i]);
JJting 0:c23e915f255b 181 i++;
JJting 0:c23e915f255b 182 }
JJting 0:c23e915f255b 183 if (i >= (sizeof(T)-1)) {
JJting 0:c23e915f255b 184 i = 0;
JJting 0:c23e915f255b 185 break;
JJting 0:c23e915f255b 186 }
JJting 0:c23e915f255b 187 }
JJting 0:c23e915f255b 188 }
JJting 0:c23e915f255b 189
JJting 0:c23e915f255b 190 void D_angle_measure()
JJting 0:c23e915f255b 191 {
JJting 0:c23e915f255b 192 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 193
JJting 0:c23e915f255b 194 if (d == 0) {
JJting 0:c23e915f255b 195 D_Angle = 0;
JJting 0:c23e915f255b 196 D_angle_old = D_angle;
JJting 0:c23e915f255b 197 d++;
JJting 0:c23e915f255b 198 } else {
JJting 0:c23e915f255b 199 D_angle_dif = D_angle - D_angle_old;
JJting 0:c23e915f255b 200 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 201 D_angle_old = D_angle;
JJting 0:c23e915f255b 202 }
JJting 0:c23e915f255b 203 }
JJting 0:c23e915f255b 204
JJting 0:c23e915f255b 205 void find_torque()
JJting 0:c23e915f255b 206 {
JJting 0:c23e915f255b 207 angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI;
JJting 0:c23e915f255b 208 torque_measured = angle_difference*ks;
JJting 0:c23e915f255b 209 }