All working, HIDScope working with BiQuads! Filter coefficients are not perfect yet.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_practPutty2 by Gerhard Berman

Files at this revision

API Documentation at this revision

Comitter:
Marieke
Date:
Tue Oct 11 11:24:52 2016 +0000
Child:
1:812a1637b6cb
Commit message:
Counts and Derivative in Putty working!

Changed in this revision

MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
QEI_DCmotors.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Tue Oct 11 11:24:52 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI_DCmotors.lib	Tue Oct 11 11:24:52 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 11 11:24:52 2016 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+#include <math.h>
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+DigitalIn encoder1A (D13); //Channel A van Encoder 1
+DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalOut led1 (D11); 
+DigitalOut led2 (D10);
+AnalogIn potMeterIn(A0);
+DigitalOut motor1DirectionPin(D7);
+PwmOut motor1MagnitudePin(D6);
+DigitalIn button1(D5);
+
+
+Serial pc(USBTX,USBRX);
+Ticker MeasureTicker, sampleT, TimeTracker;
+int counts;
+float DerivativeCounts;
+int countsPrev = 0;
+
+float referenceVelocity = 0;
+
+volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false;
+
+void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
+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);
+    //int countsPrev = 0;
+    /* QEI Encoder(D12, D13, NC, 32); // turns on encoder
+    counts = Encoder.getPulses();  // gives position
+    DerivativeCounts = ((float) counts-countsPrev)/0.01;
+    countsPrev = counts; 
+    pc.printf("Counts: %i rad/s \r\n", counts);
+    pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);*/
+}
+
+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);
+}*/
+
+int main()
+{
+ //Initialize
+ led1=0;
+ led2=0;
+ float Potmeter = potMeterIn.read();
+ MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
+ TimeTracker.attach(&TimeTracker_act, 0.1f);
+ 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 (MeasureTicker_go){
+            MeasureTicker_go=false;
+            MeasureAndControl();
+            // Encoder part
+            counts = Encoder.getPulses();  // gives position
+            DerivativeCounts = ((float) counts-countsPrev)/0.01;
+            countsPrev = counts; 
+            pc.printf("Counts: %i rad/s \r\n", counts);
+            pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
+        }
+        if (TimeTracker_go){
+            TimeTracker_go=false;
+            TimeTrackerF();
+        }
+        /*if (sampleT_go){
+            sampleT_go=false;
+            sample();
+        }*/
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 11 11:24:52 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file