2.131 test rig DAQ for MTB suspension characterization project
Dependencies: EthernetInterface ExperimentServer HX711 QEI_pmw mbed-rtos mbed
Fork of Dolphin by
Revision 7:d5f354b450a3, committed 2016-05-03
- Comitter:
- laskowsk
- Date:
- Tue May 03 17:22:31 2016 +0000
- Parent:
- 6:b7f6433cc765
- Commit message:
- This is the coding used in the 2.131 Biking Beavers test rig to characterize a mountain bikes suspension system
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 30 18:29:56 2015 +0000 +++ b/main.cpp Tue May 03 17:22:31 2016 +0000 @@ -2,139 +2,46 @@ #include "rtos.h" #include "EthernetInterface.h" #include "ExperimentServer.h" -#include "QEI.h" #include "HX711.h" -#define NUM_INPUTS 22 -#define NUM_OUTPUTS 16 -#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f) +#define NUM_INPUTS 5 +#define NUM_OUTPUTS 4 Serial pc(USBTX, USBRX); // USB Serial Terminal ExperimentServer server; // Object that lets us communicate with MATLAB Timer t; // Timer to measure elapsed time of experiment - -// Variables for q1 -float current1; -float current_des1; -float angle1; -float angle_des1; -float velocity1; -float velocity_des1; -float duty_factor1; -float angle1_init; -float max_angle1; -float dif_ang1; +AnalogIn pot(A0); // Linear Potentiometer +HX711 scale(A1, A2); +DigitalOut led(LED_BLUE); +Ticker randloop; -// Variables for q2 -float current2; -float current_des2; -float angle2; -float angle_des2; -float velocity2; -float velocity_des2; -float duty_factor2; -float angle2_init; -float max_angle2; -float dif_ang2; - + + + // Timing parameters float pwm_period_us; float current_control_period_us; float impedance_control_period_us; float exp_period; -float omega; -float phase; - -// Control parameters -float K_1; -float K_2; - -float D_1; -float D_2; - -float current_gain; float calibration_factor; -float xDesFoot; -float yDesFoot; - - -// Model parameters -float R; -float k_emf; -float nu1, nu2; -float supply_voltage; -float duty_max; - - -DigitalOut motorFwd1(D7); -DigitalOut motorRev1(D6); -AnalogIn currentSense1(A0); -PwmOut pwmOut1(D5); - -DigitalOut motorFwd2(D13); -DigitalOut motorRev2(D12); -AnalogIn currentSense2(A1); -PwmOut pwmOut2(D11); - -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 scale(A0,A1); - -Ticker currentLoop; - - -float prev_current_des1 = 0; -float prev_current_des2 = 0; -void CurrentLoop() +void randomloop() { - motorFwd1 = current_des1 >= 0; - motorRev1 = current_des1 < 0; - - current1 = currentSense1.read()*3.3f / 0.14f; //measure current - if (prev_current_des1 < 0) - current1*=-1; - - duty_factor1=(current_des1*R + current_gain*(current_des1-current1) + k_emf*velocity1)/supply_voltage; - motorRev1 = duty_factor1< 0; - motorFwd1 = duty_factor1>=0; - float absDuty1 = abs(duty_factor1); - if (absDuty1 > duty_max) { - duty_factor1 *= duty_max / absDuty1; - absDuty1= duty_max; - } - pwmOut1.write(absDuty1); - prev_current_des1 = current_des1; - - motorFwd2 = current_des2 >= 0; - motorRev2 = current_des2 < 0; - - current2 = currentSense2.read()*3.3f / 0.14f; //measure current - if (prev_current_des2 < 0) - current2*=-1; - - duty_factor2=(current_des2*R + current_gain*(current_des2-current2) + k_emf*velocity2)/supply_voltage; - - motorRev2 = duty_factor2< 0; - motorFwd2 = duty_factor2>=0; - float absDuty2 = abs(duty_factor2); - if (absDuty2 > duty_max) { - duty_factor2 *= duty_max / absDuty2; - absDuty2= duty_max; - } - pwmOut2.write(absDuty2); - prev_current_des2 = current_des2; -} +int random = rand()%100; + if (random > 85) { + led.write(0); + } + else { + led.write(1); + } +} // end randomloop int main (void) { - scale.tare(); // tare scale - encoder1.reset(); - encoder2.reset(); - + scale.tare(); // tare scale + // Link the terminal with our server and start it up server.attachTerminal(pc); server.init(); - // Continually get input from MATLAB and run experiments float input_params[NUM_INPUTS]; while(1) { @@ -143,94 +50,36 @@ current_control_period_us = input_params[1]; // Current control period in micro seconds impedance_control_period_us = input_params[2]; // Impedance control period in microseconds seconds exp_period = input_params[3]; // Experiment time in seconds - - R = input_params[4]; // Terminal resistance (Ohms) - k_emf = input_params[5]; // Back EMF Constant (V / (rad/s)) - nu1 = input_params[6]; // Friction coefficienct 1 (Nm / (rad/s)) - nu2 = input_params[7]; // Friction coefficienct 1 (Nm / (rad/s)) - supply_voltage = input_params[8]; // Power Supply Voltage (V) - - angle1_init = input_params[9]; // Initial angle for q1 (rad) - angle2_init = input_params[10];// Initial angle for q2 (rad) - - current_gain = input_params[11]; // Proportional current gain (V/A) - K_1 = input_params[12]; // Foot stiffness N/m - K_2 = input_params[13]; // Foot stiffness N/m - max_angle1 = input_params[14]; // Foot stiffness N/m + calibration_factor = input_params[4]; // calibration factor for load cell - D_1 = input_params[15]; // Foot damping N/(m/s) - D_2 = input_params[16]; // Foot damping N/(m/s) - max_angle2 = input_params[17]; // Foot damping N/(m/s) - 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); + randloop.attach_us(randomloop,current_control_period_us); + pc.printf("This is working: %4f/n", pwm_period_us); scale.setScale(calibration_factor); //Adjust to this calibration factor - - - // Attach current loop! - currentLoop.attach_us(CurrentLoop,current_control_period_us); - + + // Setup experiment t.reset(); t.start(); - - motorFwd1 = 1; - motorRev1 = 0; - pwmOut1.write(0); - - motorFwd2 = 1; - motorRev2 = 0; - pwmOut2.write(0); - - // Run experiment - while( t.read() < exp_period ) { + + // Run experiment, keep this line + while ( t.read() < exp_period ) { // Perform control loop logic - angle1 = encoder1.getPulses() *PULSE_TO_RAD; // calculate actual angle of motor - dif_ang1 = angle1 - max_angle1*sin(omega*t.read()); // calc difference of actual and desired angle - velocity1 = encoder1.getVelocity() * PULSE_TO_RAD; - - 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_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; - // take measurement from load cell - float F = scale.getGram(); // value from load cell that uses calibration factor - + float F = scale.getGram();//+ 0.116f*t.read(); // value from load cell that uses calibration factor + float Length = pot.read()*105.0f; // value from linear potentiometer * 1.05 calibration factor (105mm stroke length + float light = led.read(); + + // Form output to send to MATLAB float output_data[NUM_OUTPUTS]; output_data[0] = t.read(); - output_data[1] = angle1; - output_data[2] = velocity1; - output_data[3] = current1; - output_data[4] = current_des1; - output_data[5] = duty_factor1; - - output_data[6] = angle2; - output_data[7] = velocity2; - output_data[8] = current2; - output_data[9] = current_des2; - output_data[10]= duty_factor2; - - output_data[11] = tau_des1; - output_data[12] = tau_des2; - output_data[13] = dif_ang1; - output_data[14] = dif_ang2; - output_data[15] = F; - //output_data[16] = fy; + output_data[1] = Length; + output_data[2] = F; + output_data[3] = light; + //output_data[4] = current_des1; + //output_data[5] = duty_factor1; + //pc.printf("Output data",output_data); // Send data to MATLAB server.sendData(output_data,NUM_OUTPUTS); @@ -239,10 +88,8 @@ // Cleanup after experiment server.setExperimentComplete(); // control.detach(); - currentLoop.detach(); - pwmOut1.write(0); - pwmOut2.write(0); + randloop.detach(); } // end if - } // end while + } // end while } // end main \ No newline at end of file