Revision 2:4baa95d7bc5f, committed 2016-11-02

Comitter:

fabian101

Date:

Wed Nov 02 15:42:16 2016 +0000

Parent:

1:c155f3c67deb

Commit message:

TEST_PROGRAM

Changed in this revision

diff -r c155f3c67deb -r 4baa95d7bc5f Encoder.lib
--- a/Encoder.lib	Mon Oct 10 16:14:59 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af

diff -r c155f3c67deb -r 4baa95d7bc5f HIDScope.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Wed Nov 02 15:42:16 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#188304906687

diff -r c155f3c67deb -r 4baa95d7bc5f PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Wed Nov 02 15:42:16 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

diff -r c155f3c67deb -r 4baa95d7bc5f QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Nov 02 15:42:16 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r c155f3c67deb -r 4baa95d7bc5f main.cpp
--- a/main.cpp	Mon Oct 10 16:14:59 2016 +0000
+++ b/main.cpp	Wed Nov 02 15:42:16 2016 +0000
@@ -1,76 +1,80 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+//#include "biquadFilter.h"
 #include "math.h"
-#include "Encoder.h"
+#include "HIDScope.h"
+#include "QEI.h"
 
 // Occupied pins A1,D2,D4,D5,D12,D13
-AnalogIn potMeter1(A1);
 DigitalOut motor1_dir(D4);
 PwmOut motor1_mag(D5);
-DigitalIn button1(D2);
-Encoder motor1(D13,D12);
-AnalogIn setpoint_1(A0);
-   
-//PwmOut led(D9);
-//HIDScope scope(3);
+QEI motor1_pos(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); // D12 on A D13 on B
+
+DigitalIn button_1(D2);
+DigitalIn button_2(D3);
+AnalogIn potMeter_1(A1);
 
 MODSERIAL pc(USBTX, USBRX);
 
 float getReferenceVelocity();
-float FeedForwardControl(float);
-void SetMotor1(float);
+float feedforwardControl(float);
+void setMotor_1(float);
 bool getDirection(bool);
+float pulsesToDegrees(int);
+void test_hid_ticker();
+void initpositionGo();
 
 const float maxVelocity= 8.4; // should be 8.4 for 80 RPM 
-const float MotorGain=8.4; // unit: (rad/s)
+const float motorGain= 8.4; // unit: (rad/s)
 const int CW = 1;
 const int CCW = 0;
-bool direction = 1;
-motor1_mag.period(1.0/10000); //10kHz
 
-/*
-    float degrees_setpoint;
-    float error;
-    float output;
-    const float degrees_per_count = 90/200.0; //quick approach
-    const float ain_degrees_range = 90; //degrees over which the potmeter can control.
-    const float kp_1_range = -1;
-*/
+bool direction = 1;
+
 int main()
 {
-    while (true) {
-       float ReferenceVelocity = getReferenceVelocity(); // returns direction + speed in rad/s
-       float motorValue = FeedForwardControl(ReferenceVelocity); // returns value between -1 and 1
-       SetMotor1(motorValue); // adjust motor
-       wait(2);
+    while (button_2) {
+        
+        float ReferenceVelocity = getReferenceVelocity(); // returns direction + speed in rad/s
+        float motorValue = feedforwardControl(ReferenceVelocity); // returns value between -1 and 1
+        setMotor_1(motorValue); // adjust motor
+        //float motor1_deg = pulsesToDegrees(motor1_pos.getPulses()) % 360;
+        //pc.printf("motor1_deg: %f \n", motor1_deg);
+        int pulses = motor1_pos.getPulses();
+        pc.printf("Pulses: %d\n\r", pulses);
+        //float revolutions = float(pulses/8400.00); // 8384 or 8400, output shaft revolutions
+        //pc.printf("Revolutions: %f\n", revolutions); 
+        wait(0.002);
     }
+motor1_mag.write(0);
 }
 
 float getReferenceVelocity() // Returns reference velocity in rad/s. 
 {
     float referenceVelocity;  // in rad/s
-    float potmeter1 = potMeter1; // read out potmeter
-    // pc.printf("%f\n", potmeter1);
-    if (getDirection(direction) == CW)   { // Clockwise rotation     
+    float potmeter1 = potMeter_1.read(); // read out potmeter
+    pc.printf("%f\n", potmeter1);
+    if (getDirection(direction) == CW)   { // Clockwise rotatio
         referenceVelocity = potmeter1 * maxVelocity; 
     } 
     else { // Counterclockwise rotation      
-        referenceVelocity = -1*potMeter1 * maxVelocity;  
+        referenceVelocity = -1*potmeter1 * maxVelocity;  
     }
     pc.printf("referenceVelocity: %f\n", referenceVelocity);
     return referenceVelocity; // duty cycle compensated
 }
 
-float FeedForwardControl(float referenceVelocity)
+float feedforwardControl(float referenceVelocity)
 {
-    float motorValue = referenceVelocity / MotorGain;
+    float motorValue = referenceVelocity / motorGain;
     pc.baud(115200);
-    pc.printf("motorValue: %f\n", motorValue);
+    pc.printf("motorValue: %f\n\r", motorValue);
     return motorValue; // between -1 and 1
 }
 
-void SetMotor1(float motorValue) //  this sets the PWM and direction
+void setMotor_1(float motorValue) //  this sets the PWM and direction
 {
+
     if (motorValue >=0){ 
         motor1_dir.write(CW); 
     }
@@ -78,7 +82,7 @@
         motor1_dir.write(CCW);
     }
     int motor1_Dir = motor1_dir;
-    pc.printf("motor1_dir: %d\n", motor1_Dir);
+    pc.printf("motor1_dir: %d\n\r", motor1_Dir);
     
     if (fabs(motorValue)>1) {
         motor1_mag.write(1); // safety, magnitude max 1
@@ -87,40 +91,46 @@
         motor1_mag.write(fabs(motorValue));   //PWM Speed Control
     }
     float motor1_Mag = motor1_mag;
-    pc.printf("motor1_mag: %f\n", motor1_Mag);
-    float sensorMag = motor1_mag.getPosition();
+    pc.printf("motor1_mag: %f\n\r", motor1_Mag);
 }
 
 bool getDirection(bool Direction){
-    direction = Direction;
-    if (button1){
-   direction = !direction;
+    if (!button_1){ // pressed
+
+   return 1; 
+            
    }
-    return direction;
+   else { return 0;}
 }
- /*
-int main()
-{
+
+// not pressed is positive for long side SO going RIGHT, so DIR is 1
+// presssed is negative for long side so going left, so DIR is 0
+// not pressed is positive for short side so going up, so DIR is 1
+// pressed is negative for short side so going down, so DIR is 0
+
+/*float pulsesToLinpos(int pulses) {
+    float Linpos = ((pulses)/(X*N))*(1/PPCM );
+    return Linpos; // in Centimeters
+}
+*/
 
-    while (true) {
-        degrees_setpoint = setpoint_1 * ain_degrees_range;
-        error = degrees_setpoint-(motor1.getPosition()*degrees_per_count); //error =  setpoint - current
-        output = error*p_value*kp_1_range;
-        if(output > 0)
-        {
-            direction_1.write(true);
+/*float revToDegrees(float revolutions){
+    double degrees = revolutions*360;
+}
+*/
+
+void initpositionGo(){
+bool initposition = false;
+    while (initposition){
+        float pulseCount = motor1_pos.getPulses();
+        if ( pulseCount < 0) {
+            motor1_dir.write(CW); 
         }
-        else
-        {
-            direction_1.write(false);
+        else if (pulseCount > 0){
+            motor1_dir.write(CCW); 
         }
-        pwm_1.write(fabs(output));
-        scope.set(0,motor1.getPosition());
-        scope.set(1,error);
-        scope.set(2,fabs(output));
-        led.write(motor1.getPosition()/100.0);
-        scope.send();
-        wait(0.01);
+        else {
+        initposition = true;
+        }
     }
 }
-*/
\ No newline at end of file