Chen Wei Ting / Mbed 2 deprecated LSM9DS1_project_4_torque

read encoder and Dynamixel angle

Dependencies:   MX28 PID mbed

main.cpp@0:2dbbe7d28fab, 2018-08-04 (annotated)

Committer:
JJting
Date:
Sat Aug 04 05:55:50 2018 +0000
Revision:
0:2dbbe7d28fab
can read encoder and Dynamixel angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JJting 0:2dbbe7d28fab 1 #include "mbed.h"
JJting 0:2dbbe7d28fab 2 #include "encoder.h"
JJting 0:2dbbe7d28fab 3 #include "Mx28.h"
JJting 0:2dbbe7d28fab 4 //#include "PID.h"
JJting 0:2dbbe7d28fab 5
JJting 0:2dbbe7d28fab 6 //********************* Dynamxiel ******************************
JJting 0:2dbbe7d28fab 7 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
JJting 0:2dbbe7d28fab 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:2dbbe7d28fab 9 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
JJting 0:2dbbe7d28fab 10 #define TxPin A0
JJting 0:2dbbe7d28fab 11 #define RxPin A1
JJting 0:2dbbe7d28fab 12 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
JJting 0:2dbbe7d28fab 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:2dbbe7d28fab 14 #define PI 3.14159265f
JJting 0:2dbbe7d28fab 15 //***************************************************************
JJting 0:2dbbe7d28fab 16
JJting 0:2dbbe7d28fab 17 Serial uart(USBTX, USBRX);
JJting 0:2dbbe7d28fab 18 //Serial uart(D10,D2); // TX : D10 RX : D2 // blueteeth
JJting 0:2dbbe7d28fab 19 DigitalOut LED(A4); // check if the code is running
JJting 0:2dbbe7d28fab 20 DigitalOut led2(A5); // check if the code is running interrupt
JJting 0:2dbbe7d28fab 21 uint8_t led2f;
JJting 0:2dbbe7d28fab 22
JJting 0:2dbbe7d28fab 23 // Timer
JJting 0:2dbbe7d28fab 24 Ticker timer1;
JJting 0:2dbbe7d28fab 25 float ITR_time1 = 10000.0; // unit：us
JJting 0:2dbbe7d28fab 26 uint8_t flag;
JJting 0:2dbbe7d28fab 27
JJting 0:2dbbe7d28fab 28 // uart_tx
JJting 0:2dbbe7d28fab 29 union splitter {
JJting 0:2dbbe7d28fab 30 short j;
JJting 0:2dbbe7d28fab 31 char C[2];
JJting 0:2dbbe7d28fab 32 // C[0] is lowbyte of j, C[1] is highbyte of j
JJting 0:2dbbe7d28fab 33 };
JJting 0:2dbbe7d28fab 34 char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
JJting 0:2dbbe7d28fab 35 int i = 0;
JJting 0:2dbbe7d28fab 36
JJting 0:2dbbe7d28fab 37 // Dynamixel
JJting 0:2dbbe7d28fab 38 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
JJting 0:2dbbe7d28fab 39 int servo_cmd;
JJting 0:2dbbe7d28fab 40 int D_angle = 0;
JJting 0:2dbbe7d28fab 41 int D_angle_dif = 0;
JJting 0:2dbbe7d28fab 42 int D_Angle;
JJting 0:2dbbe7d28fab 43 int D_angle_old;
JJting 0:2dbbe7d28fab 44 unsigned short d = 0;
JJting 0:2dbbe7d28fab 45 // Find Torque
JJting 0:2dbbe7d28fab 46 double angle_difference = 0.0;
JJting 0:2dbbe7d28fab 47 float torque_measured = 0.0;
JJting 0:2dbbe7d28fab 48 float ks = 2.6393*2; //spring constant
JJting 0:2dbbe7d28fab 49 //int angle_dif = 0;
JJting 0:2dbbe7d28fab 50 float torque_ref = 0.0;
JJting 0:2dbbe7d28fab 51 float friction = 0.18f;
JJting 0:2dbbe7d28fab 52 //float friction = 0.0f;
JJting 0:2dbbe7d28fab 53 //float check = 0.0f;
JJting 0:2dbbe7d28fab 54
JJting 0:2dbbe7d28fab 55 // function
JJting 0:2dbbe7d28fab 56 void init_UART();
JJting 0:2dbbe7d28fab 57 void init_TIMER();
JJting 0:2dbbe7d28fab 58 void timer1_ITR();
JJting 0:2dbbe7d28fab 59 void uart_tx();
JJting 0:2dbbe7d28fab 60
JJting 0:2dbbe7d28fab 61 void init_DYNAMIXEL();
JJting 0:2dbbe7d28fab 62 void D_angle_measure();
JJting 0:2dbbe7d28fab 63
JJting 0:2dbbe7d28fab 64
JJting 0:2dbbe7d28fab 65 int main()
JJting 0:2dbbe7d28fab 66 {
JJting 0:2dbbe7d28fab 67 LED = 1; // darken
JJting 0:2dbbe7d28fab 68 wait_ms(500);
JJting 0:2dbbe7d28fab 69 // initial sensor
JJting 0:2dbbe7d28fab 70 init_SPI_encoder();
JJting 0:2dbbe7d28fab 71 init_encoder();
JJting 0:2dbbe7d28fab 72 init_DYNAMIXEL();
JJting 0:2dbbe7d28fab 73 // initial uart
JJting 0:2dbbe7d28fab 74 init_UART();
JJting 0:2dbbe7d28fab 75
JJting 0:2dbbe7d28fab 76 wait_ms(500);
JJting 0:2dbbe7d28fab 77
JJting 0:2dbbe7d28fab 78 led2 = 1;
JJting 0:2dbbe7d28fab 79 led2f = 0;
JJting 0:2dbbe7d28fab 80 LED = 0; // lighten
JJting 0:2dbbe7d28fab 81
JJting 0:2dbbe7d28fab 82 init_TIMER();
JJting 0:2dbbe7d28fab 83
JJting 0:2dbbe7d28fab 84 while(1) {
JJting 0:2dbbe7d28fab 85 if (flag==1) {
JJting 0:2dbbe7d28fab 86 led2 = !led2;
JJting 0:2dbbe7d28fab 87 angle_measure();
JJting 0:2dbbe7d28fab 88 D_angle_measure();
JJting 0:2dbbe7d28fab 89 uart_tx();
JJting 0:2dbbe7d28fab 90 flag = 0;
JJting 0:2dbbe7d28fab 91 }
JJting 0:2dbbe7d28fab 92 }
JJting 0:2dbbe7d28fab 93 }
JJting 0:2dbbe7d28fab 94
JJting 0:2dbbe7d28fab 95 void init_DYNAMIXEL()
JJting 0:2dbbe7d28fab 96 {
JJting 0:2dbbe7d28fab 97 dynamixelClass.torqueMode(SERVO_ID, 1);
JJting 0:2dbbe7d28fab 98 wait_ms(1);
JJting 0:2dbbe7d28fab 99 }
JJting 0:2dbbe7d28fab 100
JJting 0:2dbbe7d28fab 101 void init_UART()
JJting 0:2dbbe7d28fab 102 {
JJting 0:2dbbe7d28fab 103 uart.baud(115200);
JJting 0:2dbbe7d28fab 104 }
JJting 0:2dbbe7d28fab 105
JJting 0:2dbbe7d28fab 106 void init_TIMER()
JJting 0:2dbbe7d28fab 107 {
JJting 0:2dbbe7d28fab 108 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:2dbbe7d28fab 109 }
JJting 0:2dbbe7d28fab 110
JJting 0:2dbbe7d28fab 111 void timer1_ITR()
JJting 0:2dbbe7d28fab 112 {
JJting 0:2dbbe7d28fab 113 flag = 1;
JJting 0:2dbbe7d28fab 114 if (led2f == 5) {
JJting 0:2dbbe7d28fab 115
JJting 0:2dbbe7d28fab 116 led2f = 0;
JJting 0:2dbbe7d28fab 117 } else {
JJting 0:2dbbe7d28fab 118 led2f++;
JJting 0:2dbbe7d28fab 119 }
JJting 0:2dbbe7d28fab 120 }
JJting 0:2dbbe7d28fab 121
JJting 0:2dbbe7d28fab 122 void uart_tx()
JJting 0:2dbbe7d28fab 123 {
JJting 0:2dbbe7d28fab 124 splitter s1;
JJting 0:2dbbe7d28fab 125 splitter s2;
JJting 0:2dbbe7d28fab 126 splitter s3;
JJting 0:2dbbe7d28fab 127 splitter s4;
JJting 0:2dbbe7d28fab 128 splitter s5;
JJting 0:2dbbe7d28fab 129 splitter s6;
JJting 0:2dbbe7d28fab 130 splitter s7;
JJting 0:2dbbe7d28fab 131
JJting 0:2dbbe7d28fab 132 s1.j = angle;
JJting 0:2dbbe7d28fab 133 s2.j = Angle;
JJting 0:2dbbe7d28fab 134 s3.j = 2;
JJting 0:2dbbe7d28fab 135 // s4.j = 1;
JJting 0:2dbbe7d28fab 136 // s5.j = 3;
JJting 0:2dbbe7d28fab 137 s4.j = D_angle;
JJting 0:2dbbe7d28fab 138 s5.j = D_Angle;
JJting 0:2dbbe7d28fab 139 s6.j = 2;
JJting 0:2dbbe7d28fab 140 s7.j = 3;
JJting 0:2dbbe7d28fab 141
JJting 0:2dbbe7d28fab 142 T[2] = s1.C[0];
JJting 0:2dbbe7d28fab 143 T[3] = s1.C[1];
JJting 0:2dbbe7d28fab 144 T[4] = s2.C[0];
JJting 0:2dbbe7d28fab 145 T[5] = s2.C[1];
JJting 0:2dbbe7d28fab 146 T[6] = s3.C[0];
JJting 0:2dbbe7d28fab 147 T[7] = s3.C[1];
JJting 0:2dbbe7d28fab 148 T[8] = s4.C[0];
JJting 0:2dbbe7d28fab 149 T[9] = s4.C[1];
JJting 0:2dbbe7d28fab 150 T[10] = s5.C[0];
JJting 0:2dbbe7d28fab 151 T[11] = s5.C[1];
JJting 0:2dbbe7d28fab 152 T[12] = s6.C[0];
JJting 0:2dbbe7d28fab 153 T[13] = s6.C[1];
JJting 0:2dbbe7d28fab 154 T[14] = s7.C[0];
JJting 0:2dbbe7d28fab 155 T[15] = s7.C[1];
JJting 0:2dbbe7d28fab 156
JJting 0:2dbbe7d28fab 157 while(1) {
JJting 0:2dbbe7d28fab 158 if (uart.writeable() == 1) {
JJting 0:2dbbe7d28fab 159 uart.putc(T[i]);
JJting 0:2dbbe7d28fab 160 i++;
JJting 0:2dbbe7d28fab 161 }
JJting 0:2dbbe7d28fab 162 if (i >= (sizeof(T)-1)) {
JJting 0:2dbbe7d28fab 163 i = 0;
JJting 0:2dbbe7d28fab 164 break;
JJting 0:2dbbe7d28fab 165 }
JJting 0:2dbbe7d28fab 166 }
JJting 0:2dbbe7d28fab 167 }
JJting 0:2dbbe7d28fab 168
JJting 0:2dbbe7d28fab 169 void D_angle_measure()
JJting 0:2dbbe7d28fab 170 {
JJting 0:2dbbe7d28fab 171 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:2dbbe7d28fab 172
JJting 0:2dbbe7d28fab 173 if (d == 0) {
JJting 0:2dbbe7d28fab 174 D_Angle = 0;
JJting 0:2dbbe7d28fab 175 D_angle_old = D_angle;
JJting 0:2dbbe7d28fab 176 d++;
JJting 0:2dbbe7d28fab 177 } else {
JJting 0:2dbbe7d28fab 178 D_angle_dif = D_angle - D_angle_old;
JJting 0:2dbbe7d28fab 179 D_Angle = D_Angle + D_angle_dif;
JJting 0:2dbbe7d28fab 180 D_angle_old = D_angle;
JJting 0:2dbbe7d28fab 181 }
JJting 0:2dbbe7d28fab 182 }