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