Diff: main.cpp

Revision:

8:fe907b9a0bab

Parent:

7:e7aa4f10d1fb

Child:

9:278d25dc0ef3

--- 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