Diff: main.cpp

Revision:

29:a48b63e60a40

Parent:

28:4b22455930ff

Child:

30:4c6321941515

--- a/main.cpp	Wed Oct 25 11:24:25 2017 +0000
+++ b/main.cpp	Wed Oct 25 12:21:50 2017 +0000
@@ -8,12 +8,15 @@
 //Define objects
 AnalogIn    emg( A0 ); //EMG
 AnalogIn    emg1( A1 ); //EMG
-HIDScope    scope( 5 ); // aantal scopes dat gemaakt kan worden
+HIDScope    scope( 6 ); // aantal scopes dat gemaakt kan worden
 DigitalOut  ledB(LED_BLUE); 
 DigitalOut  ledG(LED_GREEN);
 DigitalIn TestButton(PTA4); // button naast het ledje
 DigitalIn onoff(PTC6); // button aan de andere kant
 Ticker emgSampleTicker; // Ticker voor de sample frequency
+DigitalOut motor2MagnitudePin(D5); // magnitude motor 2
+DigitalOut motor2DirectionPin(D4); // direction of the motor 2
+InterruptIn MotorOn(D10);
 
 
  int P= 200; // aantal test punten voor de moving average
@@ -22,6 +25,8 @@
  double Vvector[200]; // vector voor de Vwaarde configuratie
  double Vwaarde[2]; // vector voor waardes van V
  int x = 0; // x waarde voor de Vwaarde
+ double movmean; 
+ int MotorLock = 0; 
  
  // Filters 
 BiQuadChain bqc;
@@ -54,7 +59,7 @@
     sum = sum + A[n];
     }
     
-    double movmean = sum/P; //dit is de moving average waarde
+    movmean = sum/P; //dit is de moving average waarde
     
     // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen
     if (TestButton==0 & k<200) {
@@ -86,13 +91,47 @@
             x=0;
         }
     }
-         
+    double EMGInput =  (movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3); 
+    scope.set(5,EMGInput);  
     scope.set(2, movmean); // stuurt de moving average naar de plot
     
     scope.send();
 }
 
+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
+    referenceVelocity = ((movmean - Vwaarde[0]*3)/(Vwaarde[1] - Vwaarde[0]*3) * maxVelocity) * MotorLock; 
+    return referenceVelocity;
+}
 
+void SetMotor2(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 (fabs(motorValue)>1) motor2MagnitudePin = 1;
+        else motor2MagnitudePin = fabs(motorValue);
+}
+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 MotorLocker() {
+    if (MotorLock == 0) {
+        MotorLock = 1;
+    }
+    else if (MotorLock == 1) {
+        MotorLock = 0;
+    }
+}
 
 int main()
 {  
@@ -100,6 +139,11 @@
 ledG = 1;
 bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt
 emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz
+
  
-    while(1) {}
+    while(1) {
+        MotorOn.rise(&MotorLocker);
+        motor2DirectionPin = 1;
+        SetMotor2(FeedForwardControl(GetReferenceVelocity()));
+    }
 }
\ No newline at end of file