Diff: main.cpp

Revision:

10:4bd8ec9e79ff

Parent:

9:6cee14e0e323

Child:

11:417193f23342

--- a/main.cpp	Mon Oct 31 12:39:38 2016 +0000
+++ b/main.cpp	Mon Oct 31 12:44:23 2016 +0000
@@ -99,14 +99,99 @@
 volatile double x = 0.0;                             // beginpositie x en y
 volatile double y = 305.5;
 volatile const double pi = 3.14159265359; 
-volatile double Theta1Gear = 60.0;             // Beginpositie op 60 graden
+volatile double Speed = 10;                          // Speed with which x and y are changed, in mm/s
+volatile double Theta1Gear = 60.0;                   // Beginpositie op 60 graden
 volatile double Theta2Gear = 60.0;
 volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
 volatile double Theta2 = Theta2Gear*42/12;
 
-bool Calibration = true;                    // beginnen met calibreren
+// Stepper motor setup
+// Set constans
+volatile int stepmode = 16;                         // step mode
+volatile int steps = 150*stepmode;                  // amount of steps for 90 degree rotation = 50, full step mode: 1.8 degree per step.
+volatile int i = 0;
+volatile bool Stepper_CCW = false;
+
+// Global variable Step_State
 volatile bool Stepper_State = false;        // false = we zijn niet bezig met flippen
-volatile double Speed = 10;                 // mm/s the position is changed
+
+/////////////////////// Functtions for stepper motor /////////////////////////////////////
+// Set the Step pin to 0
+void StepperFall()
+{
+    pinStep = 0;
+}
+// Turn off stepper and power off driver board. Reset Stepper_CCS, i and Stepper_State
+void Stepper_Off()
+{
+    TickerStepper.detach();
+    pinSleep = 0;
+    i=0;
+    Stepper_State = false;
+    Stepper_CCW = false;
+}
+
+// Change direction
+void Stepper_Change_Dir()
+{
+    pinDir = true;
+    Stepper_CCW = true;
+    i = 0; 
+}
+
+// Move the motor 1 step
+void StepperRise()
+{
+    if((i<steps)&&(not Stepper_CCW))
+    {
+        pinStep=1; // Set stepper to 1
+        // attach timeout to set it back to 0, min step duration is 1.8 us
+        TimeoutStepper.attach_us(&StepperFall, 2);
+        i=i+1;
+    }
+    else if (Stepper_CCW)
+    {
+        if(i<steps)
+        {
+            pinStep=1;
+            TimeoutStepper.attach_us(&StepperFall, 2);
+            i=i+1;
+        }
+        else 
+        {
+            Stepper_Off();
+        }
+    }
+    else
+    {
+         Stepper_Change_Dir();
+    }
+}
+
+// Activate the stepper motor
+// at a constant speed
+void Stepper_On()
+{
+    // Set global variable to true
+    Stepper_State = true;
+    // Power on the board and set direction to CW
+    pinSleep = 1;
+    pinDir = false;
+    // Calc the speed of the stepper
+    int nPPS = 50*stepmode;  // was 1200, this controlls the speed, keep this times the step mode
+    float fFrequency_Hz = 1.f * nPPS;
+    float fPeriod_s = 1.0f / fFrequency_Hz;
+    // Check if the speed is too fast small and report back if that is the case
+    if (fPeriod_s < 2e-6)
+    {
+        pc.printf("\r\n ERROR: fPeriod_s too small \r\n");
+        fPeriod_s=2e-6;
+    }    
+    
+    TickerStepper.attach(&StepperRise, // void(*fptr)(void)
+                        fPeriod_s   // float t
+                        );
+}
 
 /////////////////////// functions for filters /////////////////////////////////////////////
 void sample()
@@ -199,8 +284,6 @@
     return Prev_Theta2_Gear;
 }
 
-void FunctieFlipper();      // declarate function, function discribed below
-// berekenen van de nieuwe x en y waardes
 void CalcXY () 
 {
     // calc steps in mm
@@ -208,7 +291,7 @@
     
     if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
         Stepper_State = true;     // we zijn aan het flipper dus geen andere dingen doen
-        FunctieFlipper() ;
+        //FunctieFlipper() ;
     }
     else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
         x = x - Step;
@@ -485,8 +568,8 @@
     scope.set(4,m1_MotorValue);
     scope.set(5,m2_MotorValue);
     // Set the motorvalues
-    //SetMotor(1, m1_MotorValue);
-    //SetMotor(2, m2_MotorValue);
+    // SetMotor(1, m1_MotorValue);
+    // SetMotor(2, m2_MotorValue);
     // Set motorvalues for scope
     // Send data to HIDScope
     scope.send();