Chen Wei Ting / Mbed 2 deprecated system_ID

ver1

Dependencies:   MX28 mbed

main.cpp@0:762ec6dc5696, 2018-08-29 (annotated)

Committer:
JJting
Date:
Wed Aug 29 07:30:11 2018 +0000
Revision:
0:762ec6dc5696
Child:
1:ded2ab55c7fb
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JJting 0:762ec6dc5696 1 #include "mbed.h"
JJting 0:762ec6dc5696 2 #include "encoder.h"
JJting 0:762ec6dc5696 3 #include "Mx28.h"
JJting 0:762ec6dc5696 4 //#include "PID.h"
JJting 0:762ec6dc5696 5
JJting 0:762ec6dc5696 6 //********************* Dynamxiel ******************************
JJting 0:762ec6dc5696 7 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
JJting 0:762ec6dc5696 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:762ec6dc5696 9 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
JJting 0:762ec6dc5696 10 #define TxPin A0
JJting 0:762ec6dc5696 11 #define RxPin A1
JJting 0:762ec6dc5696 12 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
JJting 0:762ec6dc5696 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:762ec6dc5696 14 #define PI 3.14159265f
JJting 0:762ec6dc5696 15 //***************************************************************
JJting 0:762ec6dc5696 16
JJting 0:762ec6dc5696 17 Serial uart(USBTX, USBRX);
JJting 0:762ec6dc5696 18 //Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth
JJting 0:762ec6dc5696 19 DigitalOut LED(A4); // check if the code is running
JJting 0:762ec6dc5696 20 DigitalOut led2(A5); // check if the code is running interrupt
JJting 0:762ec6dc5696 21 uint8_t led2f;
JJting 0:762ec6dc5696 22
JJting 0:762ec6dc5696 23 // Timer
JJting 0:762ec6dc5696 24 Ticker timer1;
JJting 0:762ec6dc5696 25 float ITR_time1 = 4000.0; // unit：us
JJting 0:762ec6dc5696 26 float Ts = ITR_time1/1000000;
JJting 0:762ec6dc5696 27 uint8_t flag;
JJting 0:762ec6dc5696 28
JJting 0:762ec6dc5696 29 // uart_tx
JJting 0:762ec6dc5696 30 union splitter {
JJting 0:762ec6dc5696 31 short j;
JJting 0:762ec6dc5696 32 char C[2];
JJting 0:762ec6dc5696 33 // C[0] is lowbyte of j, C[1] is highbyte of j
JJting 0:762ec6dc5696 34 };
JJting 0:762ec6dc5696 35 char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
JJting 0:762ec6dc5696 36 int i = 0;
JJting 0:762ec6dc5696 37
JJting 0:762ec6dc5696 38 // PID
JJting 0:762ec6dc5696 39 //PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19
JJting 0:762ec6dc5696 40 //float PIDout = 0.0f;
JJting 0:762ec6dc5696 41
JJting 0:762ec6dc5696 42 // Dynamixel
JJting 0:762ec6dc5696 43 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
JJting 0:762ec6dc5696 44 int servo_cmd;
JJting 0:762ec6dc5696 45 int row_cmd;
JJting 0:762ec6dc5696 46 int D_angle = 0;
JJting 0:762ec6dc5696 47 int D_angle_dif = 0;
JJting 0:762ec6dc5696 48 int D_Angle;
JJting 0:762ec6dc5696 49 int D_angle_old;
JJting 0:762ec6dc5696 50 unsigned short d = 0;
JJting 0:762ec6dc5696 51 // Find Torque
JJting 0:762ec6dc5696 52 double angle_difference = 0.0;
JJting 0:762ec6dc5696 53 float torque_measured = 0.0;
JJting 0:762ec6dc5696 54 float ks = 2.6393*2; //spring constant
JJting 0:762ec6dc5696 55 //int angle_dif = 0;
JJting 0:762ec6dc5696 56 //float torque_ref = 0.0;
JJting 0:762ec6dc5696 57 //float friction = 0.0f;
JJting 0:762ec6dc5696 58 //float friction = 0.18f;
JJting 0:762ec6dc5696 59 //float rate = 0.5;
JJting 0:762ec6dc5696 60 //float friction = 0.0f;
JJting 0:762ec6dc5696 61 //float check = 0.0f;
JJting 0:762ec6dc5696 62 float Angle_Dif;
JJting 0:762ec6dc5696 63 short rotation_;
JJting 0:762ec6dc5696 64
JJting 0:762ec6dc5696 65 // function
JJting 0:762ec6dc5696 66 void init_UART();
JJting 0:762ec6dc5696 67 void init_TIMER();
JJting 0:762ec6dc5696 68 void timer1_ITR();
JJting 0:762ec6dc5696 69 void uart_tx();
JJting 0:762ec6dc5696 70
JJting 0:762ec6dc5696 71 void init_DYNAMIXEL();
JJting 0:762ec6dc5696 72 void D_angle_measure();
JJting 0:762ec6dc5696 73 void find_torque();
JJting 0:762ec6dc5696 74
JJting 0:762ec6dc5696 75 int main()
JJting 0:762ec6dc5696 76 {
JJting 0:762ec6dc5696 77 LED = 1; // darken
JJting 0:762ec6dc5696 78 wait_ms(500);
JJting 0:762ec6dc5696 79 // initial sensor
JJting 0:762ec6dc5696 80 // init_SPI_encoder();
JJting 0:762ec6dc5696 81 // init_encoder();
JJting 0:762ec6dc5696 82 init_DYNAMIXEL();
JJting 0:762ec6dc5696 83 // initial uart
JJting 0:762ec6dc5696 84 init_UART();
JJting 0:762ec6dc5696 85
JJting 0:762ec6dc5696 86 wait_ms(500);
JJting 0:762ec6dc5696 87
JJting 0:762ec6dc5696 88 led2 = 1;
JJting 0:762ec6dc5696 89 // led2f = 0;
JJting 0:762ec6dc5696 90 LED = 0; // lighten
JJting 0:762ec6dc5696 91
JJting 0:762ec6dc5696 92 init_TIMER();
JJting 0:762ec6dc5696 93
JJting 0:762ec6dc5696 94 while(1) {
JJting 0:762ec6dc5696 95
JJting 0:762ec6dc5696 96 }
JJting 0:762ec6dc5696 97 }
JJting 0:762ec6dc5696 98
JJting 0:762ec6dc5696 99 void init_DYNAMIXEL()
JJting 0:762ec6dc5696 100 {
JJting 0:762ec6dc5696 101 // dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 0:762ec6dc5696 102 dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
JJting 0:762ec6dc5696 103 wait_ms(1);
JJting 0:762ec6dc5696 104 }
JJting 0:762ec6dc5696 105
JJting 0:762ec6dc5696 106 void init_UART()
JJting 0:762ec6dc5696 107 {
JJting 0:762ec6dc5696 108 uart.baud(115200);
JJting 0:762ec6dc5696 109 }
JJting 0:762ec6dc5696 110
JJting 0:762ec6dc5696 111 void init_TIMER()
JJting 0:762ec6dc5696 112 {
JJting 0:762ec6dc5696 113 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:762ec6dc5696 114 }
JJting 0:762ec6dc5696 115
JJting 0:762ec6dc5696 116 int t = 0;
JJting 0:762ec6dc5696 117 float cmd;
JJting 0:762ec6dc5696 118 float cmd_old = 0.0f;
JJting 0:762ec6dc5696 119 float f = 0.1f;
JJting 0:762ec6dc5696 120 short f_flag = 0;
JJting 0:762ec6dc5696 121 short f_change = 0;
JJting 0:762ec6dc5696 122 short f_stop = 0;
JJting 0:762ec6dc5696 123
JJting 0:762ec6dc5696 124 void timer1_ITR()
JJting 0:762ec6dc5696 125 {
JJting 0:762ec6dc5696 126 led2 = !led2;
JJting 0:762ec6dc5696 127 // angle_measure();
JJting 0:762ec6dc5696 128 D_angle_measure();
JJting 0:762ec6dc5696 129 // D_angle = dynamixelClass.readSpeed(SERVO_ID);
JJting 0:762ec6dc5696 130 cmd = 300*sin(2*PI*f*t*ITR_time1*0.001f*0.001f);
JJting 0:762ec6dc5696 131 t++;
JJting 0:762ec6dc5696 132
JJting 0:762ec6dc5696 133 if (cmd*cmd_old<0) {
JJting 0:762ec6dc5696 134 f_flag++;
JJting 0:762ec6dc5696 135 }
JJting 0:762ec6dc5696 136
JJting 0:762ec6dc5696 137 if (f_flag == 2) {
JJting 0:762ec6dc5696 138 f_change = 1;
JJting 0:762ec6dc5696 139 f_flag = 0;
JJting 0:762ec6dc5696 140 }
JJting 0:762ec6dc5696 141
JJting 0:762ec6dc5696 142 if (f_change == 1) {
JJting 0:762ec6dc5696 143
JJting 0:762ec6dc5696 144 if (f<1) {
JJting 0:762ec6dc5696 145 f += 0.01f;
JJting 0:762ec6dc5696 146 } else if (f<10) {
JJting 0:762ec6dc5696 147 f += 0.1f;
JJting 0:762ec6dc5696 148 } else {
JJting 0:762ec6dc5696 149 f += 0.2f;
JJting 0:762ec6dc5696 150 }
JJting 0:762ec6dc5696 151
JJting 0:762ec6dc5696 152 f_change = 0;
JJting 0:762ec6dc5696 153 t = 0;
JJting 0:762ec6dc5696 154 }
JJting 0:762ec6dc5696 155
JJting 0:762ec6dc5696 156 cmd_old = cmd;
JJting 0:762ec6dc5696 157
JJting 0:762ec6dc5696 158 if (f>51) {
JJting 0:762ec6dc5696 159 f = 0.1;
JJting 0:762ec6dc5696 160 f_stop = 1;
JJting 0:762ec6dc5696 161 cmd_old = 0;
JJting 0:762ec6dc5696 162 t = 0;
JJting 0:762ec6dc5696 163 }
JJting 0:762ec6dc5696 164
JJting 0:762ec6dc5696 165 servo_cmd = cmd;
JJting 0:762ec6dc5696 166
JJting 0:762ec6dc5696 167
JJting 0:762ec6dc5696 168 // 速度控制
JJting 0:762ec6dc5696 169 if (servo_cmd > 0) {
JJting 0:762ec6dc5696 170 row_cmd = servo_cmd;
JJting 0:762ec6dc5696 171 // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f;
JJting 0:762ec6dc5696 172 servo_cmd = servo_cmd;
JJting 0:762ec6dc5696 173 rotation_ = 0; // 0：Move Left
JJting 0:762ec6dc5696 174 if (servo_cmd >= 1023) {
JJting 0:762ec6dc5696 175 servo_cmd = 1023;
JJting 0:762ec6dc5696 176 row_cmd = servo_cmd;
JJting 0:762ec6dc5696 177 }
JJting 0:762ec6dc5696 178 } else {
JJting 0:762ec6dc5696 179 row_cmd = servo_cmd;
JJting 0:762ec6dc5696 180 // servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f;
JJting 0:762ec6dc5696 181 servo_cmd = -servo_cmd;
JJting 0:762ec6dc5696 182 rotation_ = 1; // 1：Move Right
JJting 0:762ec6dc5696 183 if (servo_cmd >= 1023) {
JJting 0:762ec6dc5696 184 servo_cmd = 1023;
JJting 0:762ec6dc5696 185 row_cmd = -servo_cmd;
JJting 0:762ec6dc5696 186 }
JJting 0:762ec6dc5696 187 }
JJting 0:762ec6dc5696 188
JJting 0:762ec6dc5696 189 // dynamixelClass.torque(SERVO_ID, servo_cmd);
JJting 0:762ec6dc5696 190 dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation)
JJting 0:762ec6dc5696 191 // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation)
JJting 0:762ec6dc5696 192 uart_tx();
JJting 0:762ec6dc5696 193 }
JJting 0:762ec6dc5696 194
JJting 0:762ec6dc5696 195 void uart_tx()
JJting 0:762ec6dc5696 196 {
JJting 0:762ec6dc5696 197 splitter s1;
JJting 0:762ec6dc5696 198 splitter s2;
JJting 0:762ec6dc5696 199 splitter s3;
JJting 0:762ec6dc5696 200 splitter s4;
JJting 0:762ec6dc5696 201 splitter s5;
JJting 0:762ec6dc5696 202 splitter s6;
JJting 0:762ec6dc5696 203 splitter s7;
JJting 0:762ec6dc5696 204
JJting 0:762ec6dc5696 205 s1.j = cmd;
JJting 0:762ec6dc5696 206 s2.j = D_Angle;
JJting 0:762ec6dc5696 207 s3.j = f*100;
JJting 0:762ec6dc5696 208 s4.j = f_stop*100;
JJting 0:762ec6dc5696 209 s5.j = f_flag;
JJting 0:762ec6dc5696 210 s6.j = t;
JJting 0:762ec6dc5696 211 s7.j = 1;
JJting 0:762ec6dc5696 212
JJting 0:762ec6dc5696 213 T[2] = s1.C[0];
JJting 0:762ec6dc5696 214 T[3] = s1.C[1];
JJting 0:762ec6dc5696 215 T[4] = s2.C[0];
JJting 0:762ec6dc5696 216 T[5] = s2.C[1];
JJting 0:762ec6dc5696 217 T[6] = s3.C[0];
JJting 0:762ec6dc5696 218 T[7] = s3.C[1];
JJting 0:762ec6dc5696 219 T[8] = s4.C[0];
JJting 0:762ec6dc5696 220 T[9] = s4.C[1];
JJting 0:762ec6dc5696 221 T[10] = s5.C[0];
JJting 0:762ec6dc5696 222 T[11] = s5.C[1];
JJting 0:762ec6dc5696 223 T[12] = s6.C[0];
JJting 0:762ec6dc5696 224 T[13] = s6.C[1];
JJting 0:762ec6dc5696 225 T[14] = s7.C[0];
JJting 0:762ec6dc5696 226 T[15] = s7.C[1];
JJting 0:762ec6dc5696 227
JJting 0:762ec6dc5696 228 while(1) {
JJting 0:762ec6dc5696 229 if (uart.writeable() == 1) {
JJting 0:762ec6dc5696 230 uart.putc(T[i]);
JJting 0:762ec6dc5696 231 i++;
JJting 0:762ec6dc5696 232 }
JJting 0:762ec6dc5696 233 if (i >= sizeof(T)) {
JJting 0:762ec6dc5696 234 i = 0;
JJting 0:762ec6dc5696 235 break;
JJting 0:762ec6dc5696 236 }
JJting 0:762ec6dc5696 237 }
JJting 0:762ec6dc5696 238 }
JJting 0:762ec6dc5696 239
JJting 0:762ec6dc5696 240 void D_angle_measure()
JJting 0:762ec6dc5696 241 {
JJting 0:762ec6dc5696 242 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:762ec6dc5696 243
JJting 0:762ec6dc5696 244 if (d == 0) {
JJting 0:762ec6dc5696 245 D_Angle = 0;
JJting 0:762ec6dc5696 246 D_angle_old = D_angle;
JJting 0:762ec6dc5696 247 d++;
JJting 0:762ec6dc5696 248 } else {
JJting 0:762ec6dc5696 249 D_angle_dif = D_angle - D_angle_old;
JJting 0:762ec6dc5696 250
JJting 0:762ec6dc5696 251 if (D_angle_dif > 4096/2) {
JJting 0:762ec6dc5696 252 D_angle_dif = -(4096-(D_angle_dif));
JJting 0:762ec6dc5696 253 } else if (D_angle_dif < -4096/2) {
JJting 0:762ec6dc5696 254 D_angle_dif = -(-4096-(D_angle_dif));
JJting 0:762ec6dc5696 255 } else {
JJting 0:762ec6dc5696 256 D_angle_dif = D_angle_dif;
JJting 0:762ec6dc5696 257 }
JJting 0:762ec6dc5696 258
JJting 0:762ec6dc5696 259 D_Angle = D_Angle + D_angle_dif;
JJting 0:762ec6dc5696 260 D_angle_old = D_angle;
JJting 0:762ec6dc5696 261 }
JJting 0:762ec6dc5696 262 }
JJting 0:762ec6dc5696 263
JJting 0:762ec6dc5696 264 void find_torque()
JJting 0:762ec6dc5696 265 {
JJting 0:762ec6dc5696 266
JJting 0:762ec6dc5696 267 Angle_Dif = Angle*3-D_Angle;
JJting 0:762ec6dc5696 268 angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 0:762ec6dc5696 269 // angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 0:762ec6dc5696 270
JJting 0:762ec6dc5696 271 torque_measured = angle_difference*ks;
JJting 0:762ec6dc5696 272 }