Revision 1:c155f3c67deb, committed 2016-10-10

Comitter:

fabian101

Date:

Mon Oct 10 16:14:59 2016 +0000

Parent:

0:2504d6711759

Child:

2:4baa95d7bc5f

Commit message:

pwm fixed

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/biquadFilter.lib	Mon Oct 10 16:14:59 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/tomlankhorst/code/biquadFilter/#26861979d305

--- a/main.cpp	Fri Oct 07 21:22:12 2016 +0000
+++ b/main.cpp	Mon Oct 10 16:14:59 2016 +0000
@@ -1,75 +1,126 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "math.h"
+#include "Encoder.h"
 
-AnalogIn PotMeterIn1(A1);
-DigitalOut motor1Dir(D4);
-DigitalOut motor1Mag(D5);
+// 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);
 
 MODSERIAL pc(USBTX, USBRX);
 
-bool button1 = 1; // implement button function for direction control motor
-int maxVel = 5; // should be 8.4 for 80 RPM 
 float getReferenceVelocity();
 float FeedForwardControl(float);
 void SetMotor1(float);
-const float maxVelocity= 4.2; // in rad/s of course!   
+bool getDirection(bool);
+
+const float maxVelocity= 8.4; // should be 8.4 for 80 RPM 
+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;
+*/
 int main()
 {
-    //setup();
     while (true) {
-       float ReferenceVelocity = getReferenceVelocity();
-       float motorValue = FeedForwardControl(ReferenceVelocity);
-       SetMotor1(motorValue);
-       wait(5);
+       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);
     }
 }
 
 float getReferenceVelocity() // Returns reference velocity in rad/s. 
 {
     float referenceVelocity;  // in rad/s
-    float potMeterIn1 = PotMeterIn1;
-    pc.baud(115200);
-    pc.printf("%f\n", potMeterIn1);
-    if (button1)   { // Clockwise rotation     
-        referenceVelocity = potMeterIn1 * maxVelocity; 
+    float potmeter1 = potMeter1; // read out potmeter
+    // pc.printf("%f\n", potmeter1);
+    if (getDirection(direction) == CW)   { // Clockwise rotation     
+        referenceVelocity = potmeter1 * maxVelocity; 
     } 
     else { // Counterclockwise rotation      
-        referenceVelocity = -1*potMeterIn1 * maxVelocity;  
+        referenceVelocity = -1*potMeter1 * maxVelocity;  
     }
+    pc.printf("referenceVelocity: %f\n", referenceVelocity);
     return referenceVelocity; // duty cycle compensated
 }
 
 float FeedForwardControl(float referenceVelocity)
 {
-    const float MotorGain=4.2; // unit: (rad/s) / PWM
     float motorValue = referenceVelocity / MotorGain;
-    pc.printf("%f\n", motorValue);
+    pc.baud(115200);
+    pc.printf("motorValue: %f\n", motorValue);
     return motorValue; // between -1 and 1
 }
 
 void SetMotor1(float motorValue) //  this sets the PWM and direction
 {
-    if (motorValue >=0){ motor1Dir=1; }// positive = clockwise rotation
+    if (motorValue >=0){ 
+        motor1_dir.write(CW); 
+    }
     else {
-        motor1Dir=0;
+        motor1_dir.write(CCW);
     }
-    float m1D = motor1Dir;
-    pc.printf("%f\n", m1D);
-    int intMotorValue = -1;
+    int motor1_Dir = motor1_dir;
+    pc.printf("motor1_dir: %d\n", motor1_Dir);
+    
     if (fabs(motorValue)>1) {
-        motor1Mag = 1; // safety, magnitude max 1
+        motor1_mag.write(1); // safety, magnitude max 1
     }   
     else {
-        intMotorValue = floor(fabs(motorValue)*256);    
-        motor1Mag = intMotorValue;   //PWM Speed Control
-        // motor1Mag = 1; // motor1Mag only outputs 0!!!!!!
+        motor1_mag.write(fabs(motorValue));   //PWM Speed Control
     }
-    float test = float(fabs(motorValue));
-    pc.printf("test: %f\n", test);
-    pc.printf("motorValue: %f\n", motorValue);
-    pc.printf("intMotorValue: %d\n", intMotorValue);
-    float m1M = motor1Mag;
-    pc.printf("motor1Mag: %f\n", m1M);
+    float motor1_Mag = motor1_mag;
+    pc.printf("motor1_mag: %f\n", motor1_Mag);
+    float sensorMag = motor1_mag.getPosition();
+}
+
+bool getDirection(bool Direction){
+    direction = Direction;
+    if (button1){
+   direction = !direction;
+   }
+    return direction;
 }
+ /*
+int main()
+{
+
+    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);
+        }
+        else
+        {
+            direction_1.write(false);
+        }
+        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);
+    }
+}
+*/
\ No newline at end of file