Revision 2:a64e3c37b571, committed 2017-10-11

Comitter:

DiondeGreef

Date:

Wed Oct 11 07:54:25 2017 +0000

Parent:

1:92a60278860a

Commit message:

Servomotoren compleet

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Oct 11 07:54:25 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/feed_forward.lib	Wed Oct 11 07:54:25 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/DiondeGreef/code/feed_forward/#92a60278860a

--- a/main.cpp	Tue Oct 03 15:35:26 2017 +0000
+++ b/main.cpp	Wed Oct 11 07:54:25 2017 +0000
@@ -2,88 +2,127 @@
 #include "MODSERIAL.h"
 #include "math.h"
 
-DigitalOut gpo(D0);
-DigitalOut motorDirection(D4);
-PwmOut motorSpeed(D5);
-AnalogIn potMeterIn(A1);
-InterruptIn button1(D3);
-Ticker ticker;
-
-MODSERIAL pc(USBTX, USBRX);
+DigitalOut Ledr(LED_RED);
+DigitalOut Ledg(LED_GREEN);
+DigitalOut Ledb(LED_BLUE);
+PwmOut motorSpeed(D13);
+PwmOut motorSpeed2(D12);
+InterruptIn Button1(PTC6);
+Ticker tick;
 
 
-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
-    if (button1)   {
-        // Clockwise rotation  
-        referenceVelocity = potMeterIn * maxVelocity;
-        } 
-    else {
-        // Counterclockwise rotation
-        referenceVelocity = -1*potMeterIn * maxVelocity;
-          }
-    return referenceVelocity;
-}
+enum states{Close, Open};
+
+states CurrentState = Open;
+
+int i = 0;
+int j = 0;
 
-void setMotor(float motorValue) {
-        if (motorValue >= 0) 
-        {
-            //float motor1DirectionPin1 = 1;
-            motorDirection=1;
-        }
-        else
-        {
-            //float motor1DirectionPin1 = 0;
-            motorDirection=0;
-        }
-        
-        if (fabs(motorValue)>1)
-        {
-          //float motor1MagnitudePin1 = 1;
-            motorSpeed = 1;
-         }
-         else  
-         {
-             //float motor1MagnitudePin1 = fabs(motorValue); 
-             motorSpeed = fabs(motorValue); 
-        }        
-}
-
-float FeedForwardControl(float referenceVelocity)
+void Change()
 {
-    // very simple linear feed-forward control
-    const float MotorGain=8.4; // unit: (rad/s) / PWM
-    float motorValue = referenceVelocity / MotorGain;
-    return motorValue;
+    if(CurrentState == Close){
+    CurrentState = Open;
+    } 
+    else{
+    CurrentState = Close;
+    }
 }
 
 
-void MeasureAndControl(void)
+
+void ProcessStateMachine(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);
-    setMotor(motorValue);
+    switch (CurrentState)
+        {        
+        case Close:
+            if(j <= 34){
+                motorSpeed.write(0);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 35){
+                motorSpeed.write(1);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 36){
+                motorSpeed.write(1);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 37){
+                motorSpeed.write(1);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 38){
+                motorSpeed.write(1);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 39){
+                motorSpeed.write(1);
+                motorSpeed2.write(1);
+                j = 0;
+            }
+            Ledr = 0;
+            Ledg = 1;
+            Ledb = 1;
+        break;
+        
+        case Open:
+            if(j <= 34){
+                motorSpeed.write(0);
+                motorSpeed2.write(0);
+                j++;
+            }
+            else if (j == 35){
+                motorSpeed.write(0);
+                motorSpeed2.write(1);
+                j++;
+            }
+            else if (j == 36){
+                motorSpeed.write(0);
+                motorSpeed2.write(1);
+                j++;
+            }
+            else if (j == 37){
+                motorSpeed.write(0);
+                motorSpeed2.write(1);
+                j++;
+            }
+            else if (j == 38){
+                motorSpeed.write(0);
+                motorSpeed2.write(1);
+                j++;
+            }
+            else if (j == 39){
+                motorSpeed.write(1);
+                motorSpeed2.write(1);
+                j = 0;
+            }
+            Ledr = 1;
+            Ledg = 1;
+            Ledb = 0;
+        break;
+        
+        default:
+            Ledr = 1;
+            Ledg = 0;
+            Ledb = 1;
+    }
 }
 
+
+
 int main() {
-    pc.baud(115200);
-    //ticker.attach(MeasureAndControl, 0.01);
-    while(true){
-        wait(0.1);
+    tick.attach(ProcessStateMachine, 0.0005);
+    Ledr = 1;
+    Ledg = 1;
+    Ledb = 1;
         
-        //pc.printf("%f\r\n",GetReferenceVelocity());
-        float v_ref = GetReferenceVelocity();
-        setMotor(v_ref);
-        pc.printf("%f \r\n", FeedForwardControl(v_ref));
-        motorDirection.write(motorDirection);
-        motorSpeed.write(motorSpeed);   //PWM Speed Control
+    while (true)
+    {
+        Button1.rise(&Change);
     }
-}
-    
\ No newline at end of file
+}  
\ No newline at end of file