Diff: main.cpp

Revision:

4:30d8610b63a6

Parent:

3:1ab2d2b1705f

Child:

5:931594a366b7

--- a/main.cpp	Mon Oct 17 13:39:05 2016 +0000
+++ b/main.cpp	Mon Oct 17 14:17:27 2016 +0000
@@ -2,7 +2,6 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
-#include "HIDScope.h"
 
 //Defining ports
 DigitalOut motor1DirectionPin (D4);
@@ -10,21 +9,17 @@
 DigitalIn button(D2);
 AnalogIn potmeter(A0);
 Serial pc(USBTX,USBRX);
-QEI Encoder(D12,D13,NC,32);
-//HIDScope scope(1);
+QEI encoder(D12,D13,NC,32);
 
 // Setting tickers and printers
 Ticker tick;
 Ticker callMotor;
 Ticker pos;
-Ticker encoderTicker;
 
 const float pi = 3.14159265359;
-const float ts = 1.0/1000.0;
-const int VELOCITY_CHANNEL = 0;
-
 //Get reference velocity
-float GetReferenceVelocity() {
+float GetReferenceVelocity()
+{
     // 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
@@ -42,7 +37,8 @@
     return referenceVelocity;
 }
 
-void SetMotor1(float motorValue) {
+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
@@ -65,14 +61,16 @@
     }
 }
 
-float FeedForwardControl(float referenceVelocity) {
+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) {
+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.
@@ -80,47 +78,32 @@
     float motorValue = FeedForwardControl(referenceVelocity);
     SetMotor1(motorValue);
 }
-   
-        
-float getPosition() {
-        //getting the position of the motor to take derivative to have a
-        //different velocity signal.
-        const float ts = 1.0/1000.0;
-        volatile float pulses = Encoder.getPulses();
-        volatile float revolutions = Encoder.getRevolutions();
-        volatile float position = (pulses / 2 * 32) * 360; //formula from QEI library
-    return position;
-}       
- 
-void encoderTick() {
-  int pulses = Encoder.getPulses();
-   
-   //calculate the total angle that is traveled so far.
-    double radians = (pulses / ts) * ((2*pi)/32);
+
+volatile float radians;
+volatile float velocity;
 
-    //update lastEncoderRead such that it can be used for next time
-    double lastEncoderRead = radians;
-    
-    //approximate the derivative for the angular velocity.
-    double xdif = radians - lastEncoderRead;
-    double xderiv = xdif / ts;
-   
+// position and things w/ encoder & QEI
+void getPosition() {
+  const float ts = 1.0/1000.0;
+  volatile int pulses = encoder.getPulses();
+  radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton
+  volatile float prevRadians = radians;
+  velocity = (radians - prevRadians) / ts;
+  }
+  
+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);
+    } 
     
-    pc.printf("rad=%d \n\d",radians);
-/*    send velocity to HIDScope
-    scope.set(VELOCITY_CHANNEL, radians);
-  scope.send();
-   */ 
-    }
-
-   
-        
 int main()
 {
-    encoderTicker.attach(encoderTick, 1.0/100000.0); 
     motor1MagnitudePin.period(1.0/100000.0);
+    pos.attach(getPosition,1);
     callMotor.attach(MeasureAndControl,1);
     pc.baud(115200);
-    //tick.attach(print,1);
-    while(true){}
+    tick.attach(print,1);
+    while(1);
 }
\ No newline at end of file