Chen Wei Ting
/
zerotorque_final
zero torque and encoder
Fork of LSM9DS1_project_5_zerotorque by
main.cpp
- Committer:
- JJting
- Date:
- 2018-08-31
- Revision:
- 5:131450b16ce3
- Parent:
- 4:a59512fe0f9a
File content as of revision 5:131450b16ce3:
#include "mbed.h" #include "encoder.h" #include "Mx28.h" #include "PID.h" //********************* Dynamxiel ****************************** #define SERVO_ID 0x01 // ID of which we will set Dynamixel too #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. #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps) #define TxPin A0 #define RxPin A1 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode #define PI 3.14159265f //*************************************************************** Serial uart(USBTX, USBRX); //Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth DigitalOut LED(A4); // check if the code is running DigitalOut led2(A5); // check if the code is running interrupt uint8_t led2f; // Timer Ticker timer1; float ITR_time1 = 4000.0; // unit:us float Ts = ITR_time1/1000000; uint8_t flag; // uart_tx union splitter { short j; char C[2]; // C[0] is lowbyte of j, C[1] is highbyte of j }; char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; int i = 0; // PID PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 float PIDout = 0.0f; // Dynamixel DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); int servo_cmd; int row_cmd; int D_angle = 0; int D_angle_dif = 0; int D_Angle; int D_angle_old; unsigned short d = 0; // Find Torque double angle_difference = 0.0; float torque_measured = 0.0; float ks = 2.6393*2; //spring constant //int angle_dif = 0; float torque_ref = 0.0; //float friction = 0.0f; float friction = 0.18f; float rate = 0.5; //float friction = 0.0f; //float check = 0.0f; float Angle_Dif; short rotation_; // function void init_UART(); void init_TIMER(); void timer1_ITR(); void uart_tx(); void init_DYNAMIXEL(); void D_angle_measure(); void find_torque(); int main() { LED = 1; // darken wait_ms(500); // initial sensor init_SPI_encoder(); init_encoder(); init_DYNAMIXEL(); // initial uart init_UART(); wait_ms(500); led2 = 1; // led2f = 0; LED = 0; // lighten init_TIMER(); while(1) { } } void init_DYNAMIXEL() { // dynamixelClass.torqueMode(SERVO_ID, 1); dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); wait_ms(1); } void init_UART() { uart.baud(115200); } void init_TIMER() { timer1.attach_us(&timer1_ITR, ITR_time1); } void timer1_ITR() { led2 = !led2; angle_measure(); D_angle_measure(); find_torque(); motor_pid.Compute(torque_ref, torque_measured); PIDout = motor_pid.output; // servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 servo_cmd = -PIDout; // 1023/8.4Nm = 121.7857 /* // 電流控制 if (servo_cmd > 0) { // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; servo_cmd = servo_cmd; if (servo_cmd >= 1023) servo_cmd = 1023; } else { // servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; servo_cmd = -servo_cmd + 1024; if (servo_cmd >= 2047) servo_cmd = 2047; } if (servo_cmd >= 1023) { row_cmd = -(servo_cmd-1023); } else { row_cmd = servo_cmd; } */ // 速度控制 if (servo_cmd > 0) { row_cmd = servo_cmd; // servo_cmd = servo_cmd + friction; servo_cmd = servo_cmd; rotation_ = 0; // 0:Move Left if (servo_cmd >= 1023) { servo_cmd = 1023; row_cmd = servo_cmd; } } else { row_cmd = servo_cmd; // servo_cmd = -servo_cmd - friction; servo_cmd = -servo_cmd; rotation_ = 1; // 1:Move Right if (servo_cmd >= 1023) { servo_cmd = 1023; row_cmd = -servo_cmd; } } // dynamixelClass.torque(SERVO_ID, servo_cmd); dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation) uart_tx(); } void uart_tx() { splitter s1; splitter s2; splitter s3; splitter s4; splitter s5; splitter s6; splitter s7; s1.j = torque_ref*1000; s2.j = torque_measured*1000; s3.j = Angle_Dif/4096*3600; // s3.j = servo_cmd; // s4.j = 1; // s5.j = 3; s4.j = D_Angle; s5.j = Angle*3; s6.j = row_cmd; s7.j = 1; T[2] = s1.C[0]; T[3] = s1.C[1]; T[4] = s2.C[0]; T[5] = s2.C[1]; T[6] = s3.C[0]; T[7] = s3.C[1]; T[8] = s4.C[0]; T[9] = s4.C[1]; T[10] = s5.C[0]; T[11] = s5.C[1]; T[12] = s6.C[0]; T[13] = s6.C[1]; T[14] = s7.C[0]; T[15] = s7.C[1]; while(1) { if (uart.writeable() == 1) { uart.putc(T[i]); i++; } if (i >= sizeof(T)) { i = 0; break; } } } void D_angle_measure() { D_angle = dynamixelClass.readPosition(SERVO_ID); if (d == 0) { D_Angle = 0; D_angle_old = D_angle; d++; } else { D_angle_dif = D_angle - D_angle_old; if (D_angle_dif > 4096/2) { D_angle_dif = -(4096-(D_angle_dif)); } else if (D_angle_dif < -4096/2) { D_angle_dif = -(-4096-(D_angle_dif)); } else { D_angle_dif = D_angle_dif; } D_Angle = D_Angle + D_angle_dif; D_angle_old = D_angle; } } void find_torque() { Angle_Dif = Angle*3-D_Angle; angle_difference = Angle_Dif/4096.0f*2*PI; // angle_difference = Angle_Dif/4096.0f*2*PI; torque_measured = angle_difference*ks; }