Rong Syuan Lin / Mbed 2 deprecated zerotorque_final

20181123

Dependencies:   mbed MX28 PID

main.cpp@6:f1c0b9476c88, 2018-11-23 (annotated)

Committer:
nylon0212
Date:
Fri Nov 23 04:43:52 2018 +0000
Revision:
6:f1c0b9476c88
Parent:
5:131450b16ce3 
20181123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nylon0212 6:f1c0b9476c88 1 #include "mbed.h" //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
JJting 0:c23e915f255b 2 #include "encoder.h"
JJting 0:c23e915f255b 3 #include "Mx28.h"
JJting 0:c23e915f255b 4 #include "PID.h"
nylon0212 6:f1c0b9476c88 5 #include "LSM9DS1.h"
JJting 0:c23e915f255b 6
JJting 0:c23e915f255b 7 //********************* Dynamxiel ******************************
JJting 0:c23e915f255b 8 #define SERVO_ID 0x01 // ID of which we will set Dynamixel too
JJting 0:c23e915f255b 9 #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:c23e915f255b 10 #define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps)
JJting 0:c23e915f255b 11 #define TxPin A0
JJting 0:c23e915f255b 12 #define RxPin A1
JJting 0:c23e915f255b 13 #define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
JJting 0:c23e915f255b 14 #define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
JJting 0:c23e915f255b 15 #define PI 3.14159265f
JJting 0:c23e915f255b 16
JJting 0:c23e915f255b 17 // Dynamixel
JJting 0:c23e915f255b 18 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
JJting 0:c23e915f255b 19 int servo_cmd;
JJting 1:2823a39f70a9 20 int row_cmd;
JJting 0:c23e915f255b 21 int D_angle = 0;
JJting 0:c23e915f255b 22 int D_angle_dif = 0;
JJting 0:c23e915f255b 23 int D_Angle;
JJting 0:c23e915f255b 24 int D_angle_old;
JJting 0:c23e915f255b 25 unsigned short d = 0;
JJting 0:c23e915f255b 26 // Find Torque
JJting 0:c23e915f255b 27 double angle_difference = 0.0;
JJting 0:c23e915f255b 28 float torque_measured = 0.0;
JJting 0:c23e915f255b 29 float ks = 2.6393*2; //spring constant
JJting 0:c23e915f255b 30 //int angle_dif = 0;
nylon0212 6:f1c0b9476c88 31 float torque_ref = 0.0; //可不令為0，而是以范僑芯的學習結果為input。令為0可使馬達扭力輸出為0，抵抗減速機摩擦力，使穿戴者感到跟沒有戴一樣。
JJting 1:2823a39f70a9 32 //float friction = 0.0f;
JJting 0:c23e915f255b 33 float friction = 0.18f;
JJting 1:2823a39f70a9 34 float rate = 0.5;
JJting 0:c23e915f255b 35 //float friction = 0.0f;
JJting 0:c23e915f255b 36 //float check = 0.0f;
JJting 1:2823a39f70a9 37 float Angle_Dif;
JJting 4:a59512fe0f9a 38 short rotation_;
nylon0212 6:f1c0b9476c88 39 //***************************************************************
nylon0212 6:f1c0b9476c88 40
nylon0212 6:f1c0b9476c88 41 Serial uart(USBTX, USBRX);
nylon0212 6:f1c0b9476c88 42 //Serial uart(D10,D2); // TX : D10 RX : D2 // bluetooth
nylon0212 6:f1c0b9476c88 43 AnalogIn EMG(PC_4); //
nylon0212 6:f1c0b9476c88 44 DigitalOut LED(A4); // check if the code is running
nylon0212 6:f1c0b9476c88 45 DigitalOut led2(A5); // check if the code is running interrupt
nylon0212 6:f1c0b9476c88 46 uint8_t led2f;
nylon0212 6:f1c0b9476c88 47 /*IMU*******************************************************************/
nylon0212 6:f1c0b9476c88 48 LSM9DS1 imu(D14, D15); //SDA SCL
nylon0212 6:f1c0b9476c88 49 LSM9DS1 imu2(PC_9,PA_8); //SDA SCL
nylon0212 6:f1c0b9476c88 50 void init_IMU();
nylon0212 6:f1c0b9476c88 51 int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis
nylon0212 6:f1c0b9476c88 52 int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis
nylon0212 6:f1c0b9476c88 53 float Acce_axis_data_f[6] = {0}; //_f代表經過低通濾波的資料
nylon0212 6:f1c0b9476c88 54 float Acce_axis_data_f_old[6] = {0};
nylon0212 6:f1c0b9476c88 55 float Gyro_axis_data_f[6] = {0};
nylon0212 6:f1c0b9476c88 56 float Gyro_axis_data_f_old[6] = {0};
nylon0212 6:f1c0b9476c88 57
nylon0212 6:f1c0b9476c88 58 // Timer
nylon0212 6:f1c0b9476c88 59 Ticker timer1;
nylon0212 6:f1c0b9476c88 60 float ITR_time1 = 4000.0; //timer interrupt unit：us 多少us計時一次 4毫秒
nylon0212 6:f1c0b9476c88 61 float Ts = ITR_time1/1000000; //控馬達的某個時間參數 不用理他
nylon0212 6:f1c0b9476c88 62
nylon0212 6:f1c0b9476c88 63 // PID
nylon0212 6:f1c0b9476c88 64 PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19
nylon0212 6:f1c0b9476c88 65 float PIDout = 0.0f;
nylon0212 6:f1c0b9476c88 66
nylon0212 6:f1c0b9476c88 67 // EMG
nylon0212 6:f1c0b9476c88 68 float lpf(float input, float output_old, float frequency); //low pass filter
nylon0212 6:f1c0b9476c88 69 float emg_value;
nylon0212 6:f1c0b9476c88 70 float emg_value_f;
nylon0212 6:f1c0b9476c88 71 float emg_value_f_old;
nylon0212 6:f1c0b9476c88 72 float Tf = ITR_time1/1000000; // 低通濾波的採樣週期
nylon0212 6:f1c0b9476c88 73
nylon0212 6:f1c0b9476c88 74 // uart_tx
nylon0212 6:f1c0b9476c88 75 union splitter { //將data切割為兩個byte
nylon0212 6:f1c0b9476c88 76 short j;
nylon0212 6:f1c0b9476c88 77 char C[2];
nylon0212 6:f1c0b9476c88 78 // C[0] is lowbyte of j, C[1] is highbyte of j
nylon0212 6:f1c0b9476c88 79 };
nylon0212 6:f1c0b9476c88 80 uint8_t T[40] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
nylon0212 6:f1c0b9476c88 81 // T[16]中的16是2+14，2為start btye 255 255，14為要傳7個參數到電腦，拆成high byte和low byte，所以是7*2=14
nylon0212 6:f1c0b9476c88 82 // 若要更改傳到電腦的參數個數，則改x*2=y，x為要傳的參數個數，y為char T[]裡面要有多少個0
nylon0212 6:f1c0b9476c88 83 int i = 0;
JJting 0:c23e915f255b 84
JJting 0:c23e915f255b 85 // function
JJting 0:c23e915f255b 86 void init_UART();
nylon0212 6:f1c0b9476c88 87 void uart_tx(); // 傳data到電腦
JJting 0:c23e915f255b 88 void init_TIMER();
JJting 0:c23e915f255b 89 void timer1_ITR();
JJting 0:c23e915f255b 90 void init_DYNAMIXEL();
JJting 0:c23e915f255b 91 void D_angle_measure();
JJting 0:c23e915f255b 92 void find_torque();
nylon0212 6:f1c0b9476c88 93 /**************************************************************************/
JJting 0:c23e915f255b 94 int main()
JJting 0:c23e915f255b 95 {
JJting 0:c23e915f255b 96 LED = 1; // darken
JJting 0:c23e915f255b 97 wait_ms(500);
JJting 0:c23e915f255b 98 // initial sensor
JJting 0:c23e915f255b 99 init_SPI_encoder();
JJting 0:c23e915f255b 100 init_encoder();
nylon0212 6:f1c0b9476c88 101 init_UART();
JJting 0:c23e915f255b 102 init_DYNAMIXEL();
nylon0212 6:f1c0b9476c88 103 init_IMU();
JJting 0:c23e915f255b 104 init_TIMER();
JJting 0:c23e915f255b 105
JJting 0:c23e915f255b 106 while(1) {
nylon0212 6:f1c0b9476c88 107 // EMG
nylon0212 6:f1c0b9476c88 108 //emg_value = EMG.read();
nylon0212 6:f1c0b9476c88 109 // emg_value_f = lpf(emg_value,emg_value_f_old,15);
nylon0212 6:f1c0b9476c88 110 // emg_value_f_old = emg_value_f;
JJting 0:c23e915f255b 111
nylon0212 6:f1c0b9476c88 112 // IMU
nylon0212 6:f1c0b9476c88 113 imu.readAccel();
nylon0212 6:f1c0b9476c88 114 imu.readGyro();
nylon0212 6:f1c0b9476c88 115 imu2.readAccel();
nylon0212 6:f1c0b9476c88 116 imu2.readGyro();
nylon0212 6:f1c0b9476c88 117
nylon0212 6:f1c0b9476c88 118 Acce_axis_data[0] = imu.ax*Acce_gain_x;
nylon0212 6:f1c0b9476c88 119 Acce_axis_data[1] = imu.ay*Acce_gain_y;
nylon0212 6:f1c0b9476c88 120 Acce_axis_data[2] = imu.az*Acce_gain_z;
nylon0212 6:f1c0b9476c88 121 Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
nylon0212 6:f1c0b9476c88 122 Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
nylon0212 6:f1c0b9476c88 123 Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
nylon0212 6:f1c0b9476c88 124
nylon0212 6:f1c0b9476c88 125 Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
nylon0212 6:f1c0b9476c88 126 Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
nylon0212 6:f1c0b9476c88 127 Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
nylon0212 6:f1c0b9476c88 128 Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
nylon0212 6:f1c0b9476c88 129 Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
nylon0212 6:f1c0b9476c88 130 Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
nylon0212 6:f1c0b9476c88 131
nylon0212 6:f1c0b9476c88 132 for(i=0; i<6; i++) {
nylon0212 6:f1c0b9476c88 133 Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
nylon0212 6:f1c0b9476c88 134 Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
nylon0212 6:f1c0b9476c88 135 Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
nylon0212 6:f1c0b9476c88 136 Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
JJting 0:c23e915f255b 137 }
nylon0212 6:f1c0b9476c88 138 // int16_t shit2 = imu.ax_raw;
nylon0212 6:f1c0b9476c88 139 // uart.printf("%d\n\r", shit2);
nylon0212 6:f1c0b9476c88 140 // uart.printf("x: %.4f, y: %.4f, z: %.4f\n\r", imu.ax, imu.ay, imu.az);
nylon0212 6:f1c0b9476c88 141 uart_tx();
nylon0212 6:f1c0b9476c88 142 init_TIMER();
nylon0212 6:f1c0b9476c88 143 while(1) {}
nylon0212 6:f1c0b9476c88 144
nylon0212 6:f1c0b9476c88 145 // wait_ms(500);
nylon0212 6:f1c0b9476c88 146 }
nylon0212 6:f1c0b9476c88 147 }
nylon0212 6:f1c0b9476c88 148
nylon0212 6:f1c0b9476c88 149 void init_IMU()
nylon0212 6:f1c0b9476c88 150 {
nylon0212 6:f1c0b9476c88 151 uint16_t shit;
nylon0212 6:f1c0b9476c88 152 shit = imu.begin(imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625); //imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625
nylon0212 6:f1c0b9476c88 153 // imu2.begin(imu.G_SCALE_500DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_476_BW_21,imu.A_ODR_476,imu.M_ODR_0625);
nylon0212 6:f1c0b9476c88 154 if(shit != 0x683D) {
nylon0212 6:f1c0b9476c88 155 while(1) {
nylon0212 6:f1c0b9476c88 156 uart.printf("IMU error\n\r");
nylon0212 6:f1c0b9476c88 157 }
nylon0212 6:f1c0b9476c88 158 }
nylon0212 6:f1c0b9476c88 159 uart.printf("IMU init success\n\r");
nylon0212 6:f1c0b9476c88 160
nylon0212 6:f1c0b9476c88 161 // uart.printf("%d\n\r", shit);
JJting 0:c23e915f255b 162 }
JJting 0:c23e915f255b 163
JJting 0:c23e915f255b 164 void init_DYNAMIXEL()
JJting 0:c23e915f255b 165 {
JJting 5:131450b16ce3 166 // dynamixelClass.torqueMode(SERVO_ID, 1);
nylon0212 6:f1c0b9476c88 167 dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); //Wheel:= continuously rotating; others: torque, position
JJting 0:c23e915f255b 168 wait_ms(1);
JJting 0:c23e915f255b 169 }
JJting 0:c23e915f255b 170
JJting 0:c23e915f255b 171 void init_UART()
JJting 0:c23e915f255b 172 {
JJting 0:c23e915f255b 173 uart.baud(115200);
JJting 0:c23e915f255b 174 }
JJting 0:c23e915f255b 175
JJting 0:c23e915f255b 176 void init_TIMER()
JJting 0:c23e915f255b 177 {
JJting 0:c23e915f255b 178 timer1.attach_us(&timer1_ITR, ITR_time1);
JJting 0:c23e915f255b 179 }
JJting 0:c23e915f255b 180
nylon0212 6:f1c0b9476c88 181 void timer1_ITR() //開始讀取資料
JJting 0:c23e915f255b 182 {
JJting 3:c05acc05c3bd 183 led2 = !led2;
JJting 3:c05acc05c3bd 184 angle_measure();
JJting 3:c05acc05c3bd 185 D_angle_measure();
JJting 3:c05acc05c3bd 186 find_torque();
JJting 3:c05acc05c3bd 187 motor_pid.Compute(torque_ref, torque_measured);
JJting 3:c05acc05c3bd 188 PIDout = motor_pid.output;
nylon0212 6:f1c0b9476c88 189 servo_cmd = -PIDout; // 1023/8.4Nm = 121.7857 可視情況調整負號
JJting 4:a59512fe0f9a 190 // 速度控制
JJting 4:a59512fe0f9a 191 if (servo_cmd > 0) {
JJting 4:a59512fe0f9a 192 row_cmd = servo_cmd;
JJting 5:131450b16ce3 193 // servo_cmd = servo_cmd + friction;
JJting 4:a59512fe0f9a 194 servo_cmd = servo_cmd;
JJting 4:a59512fe0f9a 195 rotation_ = 0; // 0：Move Left
JJting 4:a59512fe0f9a 196 if (servo_cmd >= 1023) {
JJting 4:a59512fe0f9a 197 servo_cmd = 1023;
JJting 4:a59512fe0f9a 198 row_cmd = servo_cmd;
JJting 4:a59512fe0f9a 199 }
JJting 4:a59512fe0f9a 200 } else {
JJting 4:a59512fe0f9a 201 row_cmd = servo_cmd;
JJting 5:131450b16ce3 202 // servo_cmd = -servo_cmd - friction;
JJting 4:a59512fe0f9a 203 servo_cmd = -servo_cmd;
JJting 4:a59512fe0f9a 204 rotation_ = 1; // 1：Move Right
JJting 4:a59512fe0f9a 205 if (servo_cmd >= 1023) {
JJting 4:a59512fe0f9a 206 servo_cmd = 1023;
JJting 4:a59512fe0f9a 207 row_cmd = -servo_cmd;
JJting 4:a59512fe0f9a 208 }
JJting 4:a59512fe0f9a 209 }
JJting 5:131450b16ce3 210
JJting 5:131450b16ce3 211 // dynamixelClass.torque(SERVO_ID, servo_cmd);
nylon0212 6:f1c0b9476c88 212 dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) 12V之下的最高轉速為45rpm，切割為等分
JJting 3:c05acc05c3bd 213 // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation)
JJting 3:c05acc05c3bd 214 uart_tx();
JJting 0:c23e915f255b 215 }
JJting 0:c23e915f255b 216
nylon0212 6:f1c0b9476c88 217 void uart_tx() //分割資料
JJting 0:c23e915f255b 218 {
JJting 0:c23e915f255b 219 splitter s1;
JJting 0:c23e915f255b 220 splitter s2;
JJting 0:c23e915f255b 221 splitter s3;
JJting 0:c23e915f255b 222 splitter s4;
JJting 0:c23e915f255b 223 splitter s5;
JJting 0:c23e915f255b 224 splitter s6;
JJting 0:c23e915f255b 225 splitter s7;
nylon0212 6:f1c0b9476c88 226 splitter s8;
nylon0212 6:f1c0b9476c88 227 splitter s9;
nylon0212 6:f1c0b9476c88 228 splitter s10;
nylon0212 6:f1c0b9476c88 229 splitter s11;
nylon0212 6:f1c0b9476c88 230 splitter s12;
nylon0212 6:f1c0b9476c88 231 splitter s13;
nylon0212 6:f1c0b9476c88 232 splitter s14;
nylon0212 6:f1c0b9476c88 233 splitter s15;
nylon0212 6:f1c0b9476c88 234 splitter s16;
nylon0212 6:f1c0b9476c88 235 splitter s17;
nylon0212 6:f1c0b9476c88 236 splitter s18;
nylon0212 6:f1c0b9476c88 237 splitter s19;
JJting 0:c23e915f255b 238
nylon0212 6:f1c0b9476c88 239 s1.j = Acce_axis_data_f[0]; // 0x6161;//
nylon0212 6:f1c0b9476c88 240 s2.j = Acce_axis_data_f[1];
nylon0212 6:f1c0b9476c88 241 s3.j = Acce_axis_data_f[2];
nylon0212 6:f1c0b9476c88 242 s4.j = Acce_axis_data_f[3];
nylon0212 6:f1c0b9476c88 243 s5.j = Acce_axis_data_f[4];
nylon0212 6:f1c0b9476c88 244 s6.j = Acce_axis_data_f[5];
nylon0212 6:f1c0b9476c88 245 s7.j = Gyro_axis_data_f[0];
nylon0212 6:f1c0b9476c88 246 s8.j = Gyro_axis_data_f[1];
nylon0212 6:f1c0b9476c88 247 s9.j = Gyro_axis_data_f[2];
nylon0212 6:f1c0b9476c88 248 s10.j = Gyro_axis_data_f[3];
nylon0212 6:f1c0b9476c88 249 s11.j = Gyro_axis_data_f[4];
nylon0212 6:f1c0b9476c88 250 s12.j = Gyro_axis_data_f[5];
nylon0212 6:f1c0b9476c88 251 s13.j = emg_value_f;
nylon0212 6:f1c0b9476c88 252 s14.j = torque_ref*1000; //即reference input指令
nylon0212 6:f1c0b9476c88 253 s15.j = torque_measured*1000; //由angle_difference*ks而來
nylon0212 6:f1c0b9476c88 254 s16.j = Angle_Dif/4096*3600;
nylon0212 6:f1c0b9476c88 255 s17.j = D_Angle;
nylon0212 6:f1c0b9476c88 256 s18.j = Angle*3; //經過減速比
nylon0212 6:f1c0b9476c88 257 s19.j = row_cmd;
JJting 0:c23e915f255b 258
JJting 0:c23e915f255b 259 T[2] = s1.C[0];
JJting 0:c23e915f255b 260 T[3] = s1.C[1];
JJting 0:c23e915f255b 261 T[4] = s2.C[0];
JJting 0:c23e915f255b 262 T[5] = s2.C[1];
JJting 0:c23e915f255b 263 T[6] = s3.C[0];
JJting 0:c23e915f255b 264 T[7] = s3.C[1];
JJting 0:c23e915f255b 265 T[8] = s4.C[0];
JJting 0:c23e915f255b 266 T[9] = s4.C[1];
JJting 0:c23e915f255b 267 T[10] = s5.C[0];
JJting 0:c23e915f255b 268 T[11] = s5.C[1];
JJting 0:c23e915f255b 269 T[12] = s6.C[0];
JJting 0:c23e915f255b 270 T[13] = s6.C[1];
JJting 0:c23e915f255b 271 T[14] = s7.C[0];
JJting 0:c23e915f255b 272 T[15] = s7.C[1];
nylon0212 6:f1c0b9476c88 273 T[16] = s8.C[0];
nylon0212 6:f1c0b9476c88 274 T[17] = s8.C[1];
nylon0212 6:f1c0b9476c88 275 T[18] = s9.C[0];
nylon0212 6:f1c0b9476c88 276 T[19] = s9.C[1];
nylon0212 6:f1c0b9476c88 277 T[20] = s10.C[0];
nylon0212 6:f1c0b9476c88 278 T[21] = s10.C[1];
nylon0212 6:f1c0b9476c88 279 T[22] = s11.C[0];
nylon0212 6:f1c0b9476c88 280 T[23] = s11.C[1];
nylon0212 6:f1c0b9476c88 281 T[24] = s12.C[0];
nylon0212 6:f1c0b9476c88 282 T[25] = s12.C[1];
nylon0212 6:f1c0b9476c88 283 T[26] = s13.C[0];
nylon0212 6:f1c0b9476c88 284 T[27] = s13.C[1];
nylon0212 6:f1c0b9476c88 285 T[28] = s14.C[0];
nylon0212 6:f1c0b9476c88 286 T[29] = s14.C[1];
nylon0212 6:f1c0b9476c88 287 T[30] = s15.C[0];
nylon0212 6:f1c0b9476c88 288 T[31] = s15.C[1];
nylon0212 6:f1c0b9476c88 289 T[32] = s16.C[0];
nylon0212 6:f1c0b9476c88 290 T[33] = s16.C[1];
nylon0212 6:f1c0b9476c88 291 T[34] = s17.C[0];
nylon0212 6:f1c0b9476c88 292 T[35] = s17.C[1];
nylon0212 6:f1c0b9476c88 293 T[36] = s18.C[0];
nylon0212 6:f1c0b9476c88 294 T[37] = s18.C[1];
nylon0212 6:f1c0b9476c88 295 T[38] = s19.C[0];
nylon0212 6:f1c0b9476c88 296 T[39] = s19.C[1];
JJting 0:c23e915f255b 297
nylon0212 6:f1c0b9476c88 298 for(int k=0;k<40;k++)
nylon0212 6:f1c0b9476c88 299 {
nylon0212 6:f1c0b9476c88 300 uart.putc(T[k]);
JJting 0:c23e915f255b 301 }
JJting 0:c23e915f255b 302 }
JJting 0:c23e915f255b 303
nylon0212 6:f1c0b9476c88 304 float lpf(float input, float output_old, float frequency)
nylon0212 6:f1c0b9476c88 305 {
nylon0212 6:f1c0b9476c88 306 float output = 0;
nylon0212 6:f1c0b9476c88 307
nylon0212 6:f1c0b9476c88 308 output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
nylon0212 6:f1c0b9476c88 309
nylon0212 6:f1c0b9476c88 310 return output;
nylon0212 6:f1c0b9476c88 311 }
nylon0212 6:f1c0b9476c88 312
JJting 0:c23e915f255b 313 void D_angle_measure()
JJting 0:c23e915f255b 314 {
JJting 0:c23e915f255b 315 D_angle = dynamixelClass.readPosition(SERVO_ID);
JJting 0:c23e915f255b 316
JJting 0:c23e915f255b 317 if (d == 0) {
JJting 0:c23e915f255b 318 D_Angle = 0;
JJting 0:c23e915f255b 319 D_angle_old = D_angle;
JJting 0:c23e915f255b 320 d++;
JJting 0:c23e915f255b 321 } else {
JJting 0:c23e915f255b 322 D_angle_dif = D_angle - D_angle_old;
JJting 3:c05acc05c3bd 323
JJting 3:c05acc05c3bd 324 if (D_angle_dif > 4096/2) {
JJting 3:c05acc05c3bd 325 D_angle_dif = -(4096-(D_angle_dif));
JJting 3:c05acc05c3bd 326 } else if (D_angle_dif < -4096/2) {
JJting 3:c05acc05c3bd 327 D_angle_dif = -(-4096-(D_angle_dif));
JJting 3:c05acc05c3bd 328 } else {
JJting 3:c05acc05c3bd 329 D_angle_dif = D_angle_dif;
JJting 3:c05acc05c3bd 330 }
JJting 3:c05acc05c3bd 331
JJting 0:c23e915f255b 332 D_Angle = D_Angle + D_angle_dif;
JJting 0:c23e915f255b 333 D_angle_old = D_angle;
JJting 0:c23e915f255b 334 }
JJting 0:c23e915f255b 335 }
JJting 0:c23e915f255b 336
JJting 0:c23e915f255b 337 void find_torque()
JJting 0:c23e915f255b 338 {
JJting 1:2823a39f70a9 339
JJting 3:c05acc05c3bd 340 Angle_Dif = Angle*3-D_Angle;
JJting 3:c05acc05c3bd 341 angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 342 // angle_difference = Angle_Dif/4096.0f*2*PI;
JJting 1:2823a39f70a9 343
JJting 0:c23e915f255b 344 torque_measured = angle_difference*ks;
JJting 1:2823a39f70a9 345 }
nylon0212 6:f1c0b9476c88 346