Revision 8:fe907b9a0bab, committed 2016-10-17

Comitter:

sjoerdbarts

Date:

Mon Oct 17 09:37:52 2016 +0000

Parent:

7:e7aa4f10d1fb

Child:

9:278d25dc0ef3

Commit message:

Initial working code

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.cpp	Mon Oct 17 09:37:52 2016 +0000
@@ -0,0 +1,160 @@
+#include "BiQuad.h"
+
+BiQuad::BiQuad() {
+    resetStateOnGainChange = true;
+    set( 1.0, 0.0, 0.0, 0.0, 0.0 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
+    resetStateOnGainChange = true;
+    set( b0, b1, b2, a1, a2 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
+    resetStateOnGainChange = true;
+    set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
+}
+
+void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts  ) {
+
+    double b0, b1, b2, bd, a1, a2;
+
+    a1 = -4.0/(N*Ts+2.0);
+    a2 = -(N*Ts-2.0)/(N*Ts+2.0);
+
+    bd = ( N*Ts+2.0 );
+
+    b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+    b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
+    b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+
+    set( b0, b1, b2, a1, a2 );
+
+};
+
+void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
+
+    B[0] = b0; B[1] = b1; B[2] = b2;
+    A[0] = a1; A[1] = a2;
+
+    if( resetStateOnGainChange )
+        wz[0] = 0; wz[1] = 0;
+
+}
+
+double BiQuad::step(double x) {
+
+    double y,w;
+
+    /* Direct form II */
+    w =      x - A[0]*wz[0] - A[1]*wz[1];
+    y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
+
+    /* Shift */
+    wz[1] = wz[0];
+    wz[0] = w;
+
+    return y;
+
+}
+
+std::vector< std::complex<double> > BiQuad::poles() {
+
+    std::vector< std::complex<double> > poles;
+
+    std::complex<double> b2(A[0]*A[0],0);
+    std::complex<double> ds = std::sqrt( b2-4*A[1] );
+
+    poles.push_back( 0.5*(-A[0]+ds) );
+    poles.push_back( 0.5*(-A[0]-ds) );
+
+    return poles;
+
+}
+
+std::vector< std::complex<double> > BiQuad::zeros() {
+
+    std::vector< std::complex<double> > zeros;
+
+    std::complex<double> b2(B[1]*B[1],0);
+    std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
+
+    zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
+    zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
+
+    return zeros;
+
+}
+
+bool BiQuad::stable() {
+    bool stable = true;
+    std::vector< std::complex<double> > ps = poles();
+    for( size_t i = 0; i < ps.size(); i++ )
+        stable = stable & ( std::abs( ps[i] ) < 1 );
+    return stable;
+}
+
+void BiQuad::setResetStateOnGainChange( bool v ){
+    resetStateOnGainChange = v;
+}
+
+BiQuadChain &BiQuadChain::add(BiQuad *bq) {
+    biquads.push_back( bq );
+    return *this;
+}
+
+BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) {
+    BiQuadChain bqc;
+    bqc.add( &bq1 ).add( &bq2 );
+    return bqc;
+}
+
+double BiQuadChain::step(double x) {
+
+    int i;
+    size_t bqs;
+
+    bqs = biquads.size();
+
+    for( i = 0; i < bqs; i++ )
+        x = biquads[i]->step( x );
+
+    return x;
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
+
+    std::vector< std::complex<double> > chain, bq;
+    int i;
+    size_t bqs;
+
+    bqs = biquads.size();
+
+    for( i = 0; i < bqs; i++ ){
+        bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
+        chain.insert( chain.end(), bq.begin(), bq.end() );
+    }
+
+    return chain;
+
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles() {
+    return poles_zeros( false );
+}
+
+std::vector< std::complex<double> > BiQuadChain::zeros() {
+    return poles_zeros( true );
+}
+
+bool BiQuadChain::stable() {
+    bool stable = true;
+    for( size_t i = 0; i < biquads.size(); i++ )
+        stable = stable & biquads[i]->stable();
+    return stable;
+}
+
+BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) {
+    add( &bq );
+    return *this;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.h	Mon Oct 17 09:37:52 2016 +0000
@@ -0,0 +1,219 @@
+#ifndef BIQUAD_BIQUAD_H
+#define BIQUAD_BIQUAD_H
+
+#include <vector>
+#include <complex>
+
+class BiQuadChain;
+
+/** BiQuad class implements a single filter
+ *
+ * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl>
+ *
+ * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is:
+ *
+ *        b0 + b1 z^-1 + b2 z^-2
+ * H(z) = ----------------------
+ *        a0 + a1 z^-1 + a2 z^-2
+ *
+ * Which is often normalized by dividing all coefficients by a0.
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include <complex>
+ *
+ * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
+ * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 );
+ * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 );
+ *
+ * BiQuadChain bqc;
+ *
+ * int main() {
+ *
+ *    // Add the biquads to the chain
+ *    bqc.add( &bq1 ).add( &bq2 );
+ *
+ *    // Find the poles of the filter
+ *    std::cout << "Filter poles" << std::endl;
+ *    std::vector< std::complex<double> > poles = bqc.poles();
+ *    for( size_t i = 0; i < poles.size(); i++ )
+ *        std::cout << "\t"  << poles[i] << std::endl;
+ *
+ *    // Find the zeros of the filter
+ *    std::cout << "Filter zeros" << std::endl;
+ *    std::vector< std::complex<double> > zeros = bqc.zeros();
+ *    for( size_t i = 0; i < poles.size(); i++ )
+ *        std::cout << "\t" << zeros[i] << std::endl;
+ *
+ *    // Is the filter stable?
+ *    std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl;
+ *
+ *    // Output the step-response of 20 samples
+ *  std::cout << "Step response 20 samples" << std::endl;
+ *  for( int i = 0; i < 20; i++ )
+ *      std::cout << "\t" << bqc.step( 1.0 ) << std::endl;
+ * }
+ * @endcode
+ *
+ * https://github.com/tomlankhorst/biquad
+ *
+ */
+class BiQuad {
+
+private:
+
+    double B[3];
+    double A[2];
+    double wz[2];
+
+    bool resetStateOnGainChange;
+
+    /**
+     * Sets the gain parameters
+     */
+    void set( double b0, double b1, double b2, double a1, double a2 );
+
+public:
+
+    /**
+     * Initialize a unity TF biquad
+     * @return BiQuad instance
+     */
+    BiQuad( );
+
+    /**
+     * Initialize a normalized biquad filter
+     * @param b0
+     * @param b1
+     * @param b2
+     * @param a1
+     * @param a2
+     * @return BiQuad instance
+     */
+    BiQuad( double b0, double b1, double b2, double a1, double a2 );
+
+    /**
+     * Initialize a biquad filter with all six coefficients
+     * @param b0
+     * @param b1
+     * @param b2
+     * @param a0
+     * @param a1
+     * @param a2
+     * @return BiQuad instance
+     */
+    BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
+
+    /**
+     * Initialize a PIDF biquad.
+     * Based on Tustin-approx (trapezoidal) of the continous time version.
+     * Behaviour equivalent to the PID controller created with the following MATLAB expression:
+     *
+     * C = pid( Kp, Ki, Kd, 1/N, Ts, 'IFormula', 'Trapezoidal', 'DFormula', 'Trapezoidal' );
+     *
+     * @param Kp    Proportional gain
+     * @param Ki    Integral gain
+     * @param Kd    Derivative gain
+     * @param N     Filter coefficient ( N = 1/Tf )
+     * @param Ts    Timestep
+     */
+    void PIDF( double Kp, double Ki, double Kd, double N, double Ts  );
+
+    /**
+     * Execute one digital timestep and return the result...
+     * @param x input of the filer
+     * @return output of the filter
+     */
+    double step( double x );
+
+    /**
+     * Return poles of the BiQuad filter
+     * @return vector of std::complex poles
+     */
+    std::vector< std::complex<double> > poles( );
+
+    /**
+     * Return zeros of the BiQuad filter
+     * @return vector of std::complex zeros
+     */
+    std::vector< std::complex<double> > zeros( );
+
+    /**
+     * Is this biquad stable?
+     * Checks if all poles lie within the unit-circle
+     * @return boolean whether the filter is stable or not
+     */
+    bool stable ();
+
+    /**
+     * Determines if the state variables are reset to zero on gain change.
+     * Can be used for changing gain parameters on the fly.
+     * @param v Value of the reset boolean
+     */
+    void setResetStateOnGainChange( bool v );
+
+};
+
+/**
+ * The BiQuadChain class implements a chain of BiQuad filters
+ */
+class BiQuadChain {
+
+private:
+    std::vector< BiQuad* > biquads;
+    std::vector< std::complex<double> > poles_zeros( bool zeros = false );
+
+public:
+
+    /**
+     * Add a BiQuad pointer to the list: bqc.add(&bq);
+     * @param bq Pointer to BiQuad instance
+     * @return Pointer to BiQuadChain
+     */
+    BiQuadChain &add( BiQuad *bq );
+
+    /**
+     * Execute a digital time step cascaded through all bq's
+     * @param x Input of the filter chain
+     * @return Output of the chain
+     */
+    double step(double x);
+
+    /**
+     * Return poles of the BiQuad filter
+     * @return vector of std::complex poles
+     */
+    std::vector< std::complex<double> > poles( );
+
+    /**
+     * Return zeros of the BiQuad filter
+     * @return vector of std::complex zeros
+     */
+    std::vector< std::complex<double> > zeros( );
+
+    /**
+     * Is this biquad-chain stable?
+     * Checks if all poles lie within the unit-circle
+     * @return boolean whether the chain is stable or not
+     */
+    bool stable ();
+
+    /**
+     * Appends a BiQuad to the chain
+     * Shorthand for .add(&bq)
+     * @param bq BiQuad
+     * @return Pointer to BiQuadChain
+     */
+    BiQuadChain &operator*( BiQuad& bq );
+
+};
+
+/**
+ * Multiply two BiQuads
+ * ... which in fact means appending them into a BiQuadChain
+ * @return BiQuadChain of the two BiQuads
+ */
+BiQuadChain operator*( BiQuad&, BiQuad& );
+
+#endif //BIQUAD_BIQUAD_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Mon Oct 17 09:37:52 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/sjoerdbarts/code/FastPWM/#ff245801a00d

--- a/HIDScope.lib	Fri Oct 07 12:28:01 2016 +0000
+++ b/HIDScope.lib	Mon Oct 17 09:37:52 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/Biorobotics_group_2/code/HIDScope/#2c5104f9f580
+https://developer.mbed.org/users/sjoerdbarts/code/HIDScope_motor_ff/#5601e1042ac2

--- a/main.cpp	Fri Oct 07 12:28:01 2016 +0000
+++ b/main.cpp	Mon Oct 17 09:37:52 2016 +0000
@@ -1,10 +1,37 @@
 #include "mbed.h"
+#include "FastPWM.h"
 #include "HIDScope.h"
 #include "QEI.h"
+#include "BiQuad.h"
 #define SERIAL_BAUD 115200  // baud rate for serial communication
- 
+
 Serial pc(USBTX,USBRX);
 
+// Setup Pins
+// Note: Pin D10 and D11 for encoder, D4-D7 for motor controller
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
+
+// Setup Buttons
+DigitalIn button1(D2);
+// InterruptIn button2(D3);
+
+// Set motor Pinouts
+DigitalOut motor1_dir(D4);
+FastPWM motor1_pwm(D5);
+//DigitalOut motor2_dir(D7);
+//FastPWM motor2_pwm(D6);
+
+// Set LED pins
+DigitalOut led(LED_RED);
+
+// Set HID scope
+HIDScope    scope(2);
+
+// Set encoder
+QEI EncoderCW(D10,D11,NC,32);
+QEI EncoderCCW(D11,D10,NC,32);
+
 // Variables counter
 int countsCW = 0;
 int countsCCW = 0;
@@ -14,49 +41,120 @@
 float degrees = 0.0;
 volatile float curr_degrees = 0.0;
 volatile float prev_degrees = 0.0;
-volatile float speed = 0.0;          // speed in degrees/s
-volatile const float T_CalculateSpeed = 0.1; // 100 Hz
+volatile float speed = 0.0; // speed in degrees/s
+
+volatile const int counts_per_rev = 8400;
+volatile const float T_CalculateSpeed = 0.001; // 1000 Hz
 
-// Set counts per revolution
-const float counts_per_rev = 4200.0;
+// BiqUadChain
+BiQuadChain bqc;
+BiQuad bq1( 3.72805e-09, 7.45610e-09, 3.72805e-09, -1.97115e+00, 9.71392e-01 );
+BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98780e+00, 9.88050e-01 );
 
-// Set encoder
-QEI EncoderCW(D12,D13,NC,32);
-QEI EncoderCCW(D13,D12,NC,32);
+
+
 
-// Print the output
-void PrintDegrees(){
-    pc.printf("\r\n Nett Pulses %i \r\n", net_counts);
-    pc.printf("\r\n Output degrees  %f \r\n", degrees);
-    pc.printf("\r\n Speed %f \r\n",speed);
+float GetReferenceVelocity()
+{
+    // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    const float maxVelocity=8.4; // in rad/s of course!   
+    float referenceVelocity;  // in rad/s
+    float button_val = button1.read();
+    if (button1.read()) {
+        // Clockwise rotation
+        referenceVelocity = pot1.read()*maxVelocity;
     }
-
-// Calculate the speed
-void CalculateSpeed() {
-    curr_degrees = degrees;
-    speed = (curr_degrees-prev_degrees)/T_CalculateSpeed; 
-    prev_degrees = curr_degrees;
+    else {
+        // Counterclockwise rotation      
+        referenceVelocity = -1*pot1.read()*maxVelocity;
+    }  
+    return referenceVelocity;
 }
 
-int main()
+float FeedForwardControl(float referenceVelocity)
+{
+    // very simple linear feed-forward control
+    const float MotorGain=8.4; // unit: (rad/s) / PWM
+    float motorValue = referenceVelocity / MotorGain;
+    pc.printf("\r\n RefVel = %f \r\n",motorValue); 
+    return motorValue;
+}
+
+void SetMotor1(float motorValue)
 {
-    pc.baud(SERIAL_BAUD);
-    pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
-    
-    // Set ticker for serial communication of counts and degrees
-    Ticker PrintDegreesTicker;
-    PrintDegreesTicker.attach(&PrintDegrees,0.1);
-    
-    // Set ticker for speed calculation
-    Ticker CalculateSpeedTicker;
-    CalculateSpeedTicker.attach(CalculateSpeed,T_CalculateSpeed);
-    
-    // count the CW and CCW counts and calculate the output degrees
-    while(true){
+    // 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){
+        motor1_dir=1;
+    }
+    else{
+        motor1_dir=0;
+        pc.printf("\r\n MOTORDIR = 0 \r\n");
+    }
+    if (fabs(motorValue)>1){
+        motor1_pwm.write(1);
+    }
+    else{
+        motor1_pwm.write(fabs(motorValue));
+    }
+}
+
+void MeasureAndControl(void)
+{
+    // 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 BlinkLed(){
+    led = not led;
+}
+
+void CalculateSpeed() {
     countsCW = EncoderCW.getPulses();
     countsCCW= EncoderCCW.getPulses();
     net_counts=countsCW-countsCCW;
     degrees=(net_counts*360.0)/counts_per_rev;
     
+    curr_degrees = degrees;
+    speed = (curr_degrees-prev_degrees)/T_CalculateSpeed; 
+    prev_degrees = curr_degrees;
+    
+    //scope.set(0, degrees);
+    scope.set(0, speed);
+    double speed_filtered = bqc.step(speed);
+    scope.set(1,speed_filtered);
+    scope.send();
+}
+
+int main(){
+    // Set baud connection with PC
+    pc.baud(SERIAL_BAUD);
+    pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
+    
+    // Setup Blinking LED
+    led = 1;
+    Ticker TickerBlinkLed;
+    TickerBlinkLed.attach(BlinkLed,0.5);
+    
+    // Set motor PWM speeds
+    motor1_pwm.period(1.0/1000);
+    // motor2_pwm.period(1.0/1000);
+    
+    Ticker CalculateSpeedTicker;
+    CalculateSpeedTicker.attach(&CalculateSpeed,T_CalculateSpeed);
+    
+    // Setup Biquad
+    bqc.add(&bq1).add(&bq2);
+    
+    // MeasureAndControl as fast as possible
+    while(true){
+        MeasureAndControl();
     }
 }
\ No newline at end of file

--- a/mbed.bld	Fri Oct 07 12:28:01 2016 +0000
+++ b/mbed.bld	Mon Oct 17 09:37:52 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/8ed44a420e5c
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file