Chen Wei Ting
ver1
Diff: main.cpp
- Revision:
- 0:762ec6dc5696
- Child:
- 1:ded2ab55c7fb
diff -r 000000000000 -r 762ec6dc5696 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 29 07:30:11 2018 +0000
@@ -0,0 +1,272 @@
+#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(7.2, 0, 0.13, 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);
+}
+
+int t = 0;
+float cmd;
+float cmd_old = 0.0f;
+float f = 0.1f;
+short f_flag = 0;
+short f_change = 0;
+short f_stop = 0;
+
+void timer1_ITR()
+{
+ led2 = !led2;
+// angle_measure();
+ D_angle_measure();
+// D_angle = dynamixelClass.readSpeed(SERVO_ID);
+ cmd = 300*sin(2*PI*f*t*ITR_time1*0.001f*0.001f);
+ t++;
+
+ if (cmd*cmd_old<0) {
+ f_flag++;
+ }
+
+ if (f_flag == 2) {
+ f_change = 1;
+ f_flag = 0;
+ }
+
+ if (f_change == 1) {
+
+ if (f<1) {
+ f += 0.01f;
+ } else if (f<10) {
+ f += 0.1f;
+ } else {
+ f += 0.2f;
+ }
+
+ f_change = 0;
+ t = 0;
+ }
+
+ cmd_old = cmd;
+
+ if (f>51) {
+ f = 0.1;
+ f_stop = 1;
+ cmd_old = 0;
+ t = 0;
+ }
+
+ servo_cmd = cmd;
+
+
+ // 速度控制
+ if (servo_cmd > 0) {
+ row_cmd = servo_cmd;
+ // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f;
+ 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 + ((torque_ref)*rate+friction)*121.78f;
+ 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 = cmd;
+ s2.j = D_Angle;
+ s3.j = f*100;
+ s4.j = f_stop*100;
+ s5.j = f_flag;
+ s6.j = t;
+ 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;
+}