Diff: main.cpp

Revision:

0:43160ef59f9f

Child:

1:ace33633653b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 14 14:18:14 2016 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include <math.h>
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+
+//set pins
+DigitalIn encoder1A (D13); //Channel A van Encoder 1
+DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalOut led1 (D11); 
+DigitalOut led2 (D10);
+AnalogIn potMeter1(A0);
+AnalogIn potMeter2(A1);
+DigitalOut motor1DirectionPin(D7);
+PwmOut motor1MagnitudePin(D6);
+DigitalIn button1(D5);
+
+//set settings
+Serial pc(USBTX,USBRX);
+Ticker MeasureTicker, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker;
+HIDScope    scope(2);
+
+//set datatypes
+int counts;
+double DerivativeCounts;
+int countsPrev = 0;
+float referenceVelocity = 0;
+double bqcDerivativeCounts = 0;
+const double PI = 3.141592653589793;
+double Potmeter1 = potMeter1.read();
+double Potmeter2 = potMeter2.read();
+const int cw = 1;
+const int ccw = 0;
+
+//define encoder counts and degrees
+QEI Encoder(D12, D13, NC, 32); // turns on encoder
+int counts_per_revolution = 4200;
+const double gear_ratio = 131;
+const double resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian
+
+//set BiQuad
+BiQuadChain bqc;
+BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
+BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
+
+//set go-Ticker settings
+volatile bool MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false;
+void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags
+void PTicker_act(){PTicker_go=true;};
+void MotorControllerTicker_act(){MotorControllerTicker_go=true;};
+//void TimeTracker_act(){TimeTracker_go=true;};
+//void sampleT_act(){sampleT_go=true;};
+
+/*
+float GetReferenceVelocity()
+{
+    // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    const float maxVelocity = 8.4; // in rad/s of course!
+    if (button1 == 0){
+        led1=1;
+        led2=0;
+        // Counterclockwise rotation
+        referenceVelocity = potMeterIn * maxVelocity;  
+    } 
+    else {   
+        led1=0;
+        led2=1;
+        // Clockwise rotation
+        referenceVelocity = -1*potMeterIn * maxVelocity;
+    }
+    return referenceVelocity;
+}
+
+float FeedForwardControl(float referenceVelocity)
+{
+    // very simple linear feed-forward control
+    const float MotorGain=8.4; // unit: (rad/s) / PWM
+    float motorValue = referenceVelocity / MotorGain;
+    return motorValue;
+}
+
+void SetMotor1(float motorValue)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) motor1DirectionPin=1;
+        else motor1DirectionPin=0;
+    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue);
+}
+
+void MeasureAndControl()
+{
+    // This function measures the potmeter position, extracts a
+    // reference velocity from it, and controls the motor with 
+    // a simple FeedForward controller. Call this from a Ticker.
+    float referenceVelocity = GetReferenceVelocity();
+    float motorValue = FeedForwardControl(referenceVelocity);
+    SetMotor1(motorValue);
+}
+
+void TimeTrackerF(){
+     wait(1);   
+     float Potmeter = potMeterIn.read();
+     pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
+     pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
+     //pc.printf("Counts: %i rad/s \r\n", counts);
+     //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
+}
+
+void sample()
+{
+    int countsPrev = 0;
+    QEI Encoder(D12, D13, NC, 32);
+    counts = Encoder.getPulses();  // gives position
+    //scope.set(0,counts);
+    DerivativeCounts = (counts-countsPrev)/0.001;
+    //scope.set(1,DerivativeCounts);
+    countsPrev = counts; 
+    //scope.send();
+    pc.printf("Counts: %i rad/s \r\n", counts);
+    pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
+}
+
+void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
+    //double in=DerivativeCounts();
+    bqcDerivativeCounts=bqc.step(DerivativeCounts);
+    //return(bqcDerivativeCounts);
+    }
+ */   
+void MeasureP(){
+    double ref_position = Potmeter1;                //reference position from potmeter
+    int counts = Encoder.getPulses();           // gives position    
+    double position = counts/resolution;         //position in radians
+    double rotation = ref_position-position;     //rotation is 'position error' in radians
+    double movement = rotation/(2*PI);  //movement in rotations
+    double Kp = Potmeter2;
+    }
+    
+double P(double rotation, double Kp){
+    double P_output = Kp*movement;
+    return P_output;
+    }
+    
+void MotorController(){
+    double output = P(rotation, Kp);
+    
+    if(rotation>0){
+        motor1DirectionPin.write(cw);
+        motor1MagnitudePin.write(output);
+        }
+    if(rotation<0){
+        motor1DirectionPin.write(ccw);
+        motor1MagnitudePin.write(-output);
+        }
+    }
+
+int main()
+{
+ //Initialize
+ led1=0;
+ led2=0;
+ float Potmeter = potMeterIn.read();
+ MeasureP.attach(&MeasureP_act, 0.01f); 
+ P.attach(&P_act, 0.01f);
+ MotorController.attach(&MotorController_act, 0.01f);
+ pc.baud(115200);  
+ QEI Encoder(D12, D13, NC, 32); // turns on encoder
+ //sampleT.attach(&sampleT_act, 0.1f);
+ //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
+ //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
+ 
+ while(1)
+    {
+        if (MeasureP_go){
+            MeasureP_go=false;
+            MeasureP();
+            // Encoder part
+            /*
+            counts = Encoder.getPulses();  // gives position
+            DerivativeCounts = ((double) counts-countsPrev)/0.01;  
+            
+            scope.set(0,counts);
+            scope.set(1,DerivativeCounts);
+            //scope.set(1,bqcDerivativeCounts);
+            scope.send();
+            countsPrev = counts;
+            //pc.printf("Counts: %i rad/s \r\n", counts);
+            //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
+            */
+        }
+        
+        if (PTicker_go){
+            PTicker_go=false;
+            P();
+        }
+        if (MotorControllerTicker_go){
+            MotorControllerTicker_go=false;
+            MotorController();
+        /*if (TimeTracker_go){
+            TimeTracker_go=false;
+            TimeTrackerF();
+        }
+        if (sampleT_go){
+            sampleT_go=false;
+            sample();
+        }*/
+    }
+}
\ No newline at end of file