![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 0:c23e915f255b
- Child:
- 1:2823a39f70a9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Aug 05 13:15:56 2018 +0000 @@ -0,0 +1,209 @@ +#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 = 10000.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(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13 +float PIDout = 0.0f; + +// Dynamixel +DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); +int servo_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.18f; +float rate = 0.8; +//float friction = 0.0f; +//float check = 0.0f; + +// 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) { + if (flag==1) { + 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 + + if (servo_cmd > 0) { + servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; + if (servo_cmd >= 1023) + servo_cmd = 1023; + } else { + servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; + if (servo_cmd >= 2047) + servo_cmd = 2047; + } +// dynamixelClass.torque(SERVO_ID, servo_cmd); + uart_tx(); + flag = 0; + } + } +} + +void init_DYNAMIXEL() +{ + dynamixelClass.torqueMode(SERVO_ID, 1); + wait_ms(1); +} + +void init_UART() +{ + uart.baud(115200); +} + +void init_TIMER() +{ + timer1.attach_us(&timer1_ITR, ITR_time1); +} + +void timer1_ITR() +{ + flag = 1; + if (led2f == 5) { + + led2f = 0; + } else { + led2f++; + } +} + +void uart_tx() +{ + splitter s1; + splitter s2; + splitter s3; + splitter s4; + splitter s5; + splitter s6; + splitter s7; + + s1.j = angle; + s2.j = Angle; + s3.j = 2; +// s4.j = 1; +// s5.j = 3; + s4.j = D_angle; + s5.j = D_Angle; + s6.j = torque_measured*1000; + s7.j = 3; + + 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)-1)) { + 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; + D_Angle = D_Angle + D_angle_dif; + D_angle_old = D_angle; + } +} + +void find_torque() +{ + angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI; + torque_measured = angle_difference*ks; +} \ No newline at end of file