File content as of revision 8:fe907b9a0bab:

#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;
int net_counts = 0;

// Variables degrees, and speed
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 int counts_per_rev = 8400;
volatile const float T_CalculateSpeed = 0.001; // 1000 Hz

// 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 );




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;
    }
    else {
        // Counterclockwise rotation      
        referenceVelocity = -1*pot1.read()*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;
    pc.printf("\r\n RefVel = %f \r\n",motorValue); 
    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){
        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();
    }
}