Diff: main.cpp

Revision:

6:6bc6ce1fe94e

Parent:

5:931594a366b7

Child:

7:742b1969f6c9

--- a/main.cpp	Wed Oct 19 12:21:58 2016 +0000
+++ b/main.cpp	Thu Oct 20 10:15:28 2016 +0000
@@ -4,117 +4,79 @@
 #include "math.h"
 #include "HIDScope.h"
 
-//Defining ports
+
+//*****************Defining ports********************
 DigitalOut motor1DirectionPin (D4);
 PwmOut motor1MagnitudePin(D5);
 DigitalIn button(D2);
-AnalogIn potmeter(A0);
 Serial pc(USBTX,USBRX);
 QEI encoder(D12,D13,NC,32);
 HIDScope scope(1);
 
-// Setting tickers and printers
-Ticker tick;
+//*******************Setting tickers and printers*******************
 Ticker callMotor;
-Ticker pos;
+Ticker angPos;
+Ticker t1;
+
+//**************Go flags********************************************
+volatile bool fn1_go = false;
+void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
+
+//***************Global Variables***********************************
+const double pi = 3.14159265359;
+const double ts = 1.0/1000.0;
+const int velocityChannel = 0;
+volatile double radians =0.0;
+const double transmissionShoulder =94.4/40.2;
+const double transmissionElbow = 1.0;
+//*****************Angles Arms***********************
 
-const float pi = 3.14159265359;
-const float ts = 1.0/1000.0;
-const int velocityChannel = 0;
+double O1=1.7633;
+double O2=2.0915;
+double O3=1.8685;
+double O4=1.1363;
+double O5=2.3960;
+double O6=2.0827;
+double B1=1.3551;
+double B2=0.5964;
+double B3=0.06652;
+double B4=0.0669;
+double B5=1.7462;
+double B6=-0.8994;
 
-//Get reference velocity
-float GetReferenceVelocity()
+//**********functions******************************
+void getAngPosition() 
+{   
+    volatile int pulses = encoder.getPulses();
+    radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
+    scope.set(velocityChannel,radians);
+    scope.send();
+}
+  
+void control(double radians)
 {
-    // Returns reference velocity in rad/s.
-    // Positive value means clockwise rotation.
-    const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
-    volatile float referenceVelocity;  // in rad/s
-    if (button) //nog even kijken voor wanneer die + en - is
-    {
-        // Clockwise rotation
-        referenceVelocity = potmeter * maxVelocity;
-    } 
-    else
-    {
-        // Counterclockwise rotation
-        referenceVelocity = -1*potmeter * maxVelocity;
-    }
-    return referenceVelocity;
-}
-
-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)
+if(radians>=(-2*pi))
         {
-            motor1DirectionPin=1;
+        motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
         }
     else
         {
-            motor1DirectionPin=0;
+        motor1MagnitudePin=0.0;    
         }
-    if (fabs(motorValue)>1) 
-    {
-        motor1MagnitudePin = 1;
-    }
-    else
-    {
-        motor1MagnitudePin = 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 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);
-}
-
-volatile float radians;
-volatile float velocity;
-volatile float prevRadians = 0;
-
-// position and things w/ encoder & QEI
-void getPosition() {
-  volatile int pulses = encoder.getPulses();
-  radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton
-  volatile float difference = radians - prevRadians;
-  velocity = difference / ts;
-  
-  scope.set(velocityChannel,velocity);
-  scope.send();
-  
-  prevRadians = radians;
-  }
-  
-void print() {
-    pc.printf("Motor value = %f \r\n", FeedForwardControl(GetReferenceVelocity()));
-    pc.printf("Reference Velocity = %f \r\n", GetReferenceVelocity());
-    pc.printf("Radians = %f \r\n", radians);
-    pc.printf("Velocity = %f \r\n", velocity);
-    } 
-    
+//****************MAIN FUNCTION*********************************
 int main()
 {
-    encoder.reset();    //not entirely sure if necessary
-    motor1MagnitudePin.period(1.0/100000.0);
-    pos.attach(getPosition,ts);
-    callMotor.attach(MeasureAndControl,1);
+    angPos.attach(&getAngPosition,ts);
+    t1.attach(&fn1_activate, 0.0001f);
+    while(true)
+        if(fn1_go)
+        {
+        fn1_go = false;
+        control(radians);
+        }
+
+
     pc.baud(115200);
-    tick.attach(print,1);
-    while(1);
+    while(true);
 }
\ No newline at end of file