![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2.74 Bio-Inspired Robotics robot for LGO group. This is the vertical dolphin tail
Dependencies: EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed
Fork of experiment_example by
Diff: main.cpp
- Revision:
- 6:b7f6433cc765
- Parent:
- 4:300ced917633
--- a/main.cpp Wed Nov 25 20:25:20 2015 +0000 +++ b/main.cpp Mon Nov 30 18:29:56 2015 +0000 @@ -5,8 +5,8 @@ #include "QEI.h" #include "HX711.h" -#define NUM_INPUTS 21 -#define NUM_OUTPUTS 17 +#define NUM_INPUTS 22 +#define NUM_OUTPUTS 16 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f) Serial pc(USBTX, USBRX); // USB Serial Terminal @@ -36,12 +36,6 @@ float angle2_init; float max_angle2; float dif_ang2; - -// Fixed kinematic parameters -const float l_OA=.010; -const float l_OB=.040; -const float l_AC=.095; -const float l_DE=.095; // Timing parameters float pwm_period_us; @@ -59,6 +53,7 @@ float D_2; float current_gain; +float calibration_factor; float xDesFoot; float yDesFoot; @@ -84,9 +79,11 @@ QEI encoder1(D3,D4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding QEI encoder2(D9,D10, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding -//HX711 load_cell(D0,D1,uint8_t 120); +HX711 scale(A0,A1); + Ticker currentLoop; + float prev_current_des1 = 0; float prev_current_des2 = 0; void CurrentLoop() @@ -130,7 +127,7 @@ } int main (void) { - + scale.tare(); // tare scale encoder1.reset(); encoder2.reset(); @@ -167,26 +164,12 @@ duty_max = input_params[18]; // Maximum duty factor omega = input_params[19]; // Oscillation freq of the tail phase = input_params[20]; // phase difference between two motors - + calibration_factor = input_params[21]; // calibration factor for load cell + pwmOut1.period_us(pwm_period_us); pwmOut2.period_us(pwm_period_us); - float m1 =.02 + .210; - float m2 =.0225; - float m3 = .004; - float m4 = .017; - //float I1 = 45.389 * 10^-6; - //float I2 = 22.918 * 10^-6; - //float I3 = 3.2570 * 10^-6; - //float I4 = 22.176 * 10^-6; - float l_OA=.011; - float l_OB=.042; - float l_AC=.096; - float l_DE=.096; - float l_O_m1=0.0364; - float l_B_m2=0.040; - float l_A_m3=1/2 * l_AC; - float l_C_m4=1/2 * (l_DE+ l_OB-l_OA); - float g = 9.81; + scale.setScale(calibration_factor); //Adjust to this calibration factor + // Attach current loop! currentLoop.attach_us(CurrentLoop,current_control_period_us); @@ -211,18 +194,21 @@ dif_ang1 = angle1 - max_angle1*sin(omega*t.read()); // calc difference of actual and desired angle velocity1 = encoder1.getVelocity() * PULSE_TO_RAD; - angle2 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor + angle2 = encoder2.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor dif_ang2 = angle2 - max_angle2*sin(omega*t.read()+phase); // calc difference of actual and desired angle velocity2 = encoder2.getVelocity() * PULSE_TO_RAD; // Forward Kinematics float tau_des1 = (-K_1*dif_ang1-D_1*velocity1+nu1*velocity1); - float tau_des2 = (-K_1*dif_ang2-D_1*velocity2+nu2*velocity2); + float tau_des2 = (-K_2*dif_ang2-D_2*velocity2+nu2*velocity2); // Set desired currents current_des1 = tau_des1/k_emf;//(-K_xx*angle1-D_xx*velocity1+nu1*velocity1)/k_emf; - current_des2 = tau_des2/k_emf; //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf; + current_des2 = tau_des2/k_emf; //(-K_yy*angle2-D_yy*velocity2+nu2*velocity2)/k_emf; + + // take measurement from load cell + float F = scale.getGram(); // value from load cell that uses calibration factor // Form output to send to MATLAB float output_data[NUM_OUTPUTS]; @@ -243,7 +229,7 @@ output_data[12] = tau_des2; output_data[13] = dif_ang1; output_data[14] = dif_ang2; - //output_data[15] = fx; + output_data[15] = F; //output_data[16] = fy; // Send data to MATLAB