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

diff -r b7f6433cc765 -r d5f354b450a3 main.cpp
--- 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