Diff: main.cpp

Revision:

2:6ca30ccec353

Parent:

1:b862262a9d14

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Mon Oct 28 16:31:37 2019 +0000
@@ -1,23 +1,163 @@
 #include "mbed.h"
+#include "math.h"
+#include "encoder.h"
+#include "QEI.h"
 //#include "HIDScope.h"
-//#include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+// ---- Alles met een 1(of geen getal) gaat over motor 1----
+// ---- Alles met een 2 gaat over motor 2---- 
+// ------ Hardware Interfaces -------
+ 
+const PinName motor1dir = D7; //direction1 (cw,ccw)
+const PinName motor1PWM = D6; //speed1
+const PinName motor2PWM = D5; //speed2
+const PinName motor2dir = D4; //direction2 (cw,ccw)
+ 
+DigitalOut motor1direction(motor1dir);
+PwmOut motor1control(motor1PWM);
+PwmOut motor2control(motor2PWM);
+DigitalOut motor2direction(motor2dir);
+ 
+const PinName button1name = D10; 
+const PinName pot1name = A0;
+const PinName button2name = D11;
+const PinName pot2name = A1;
+InterruptIn button1(button1name);    //had ook direct gekund maar he
+AnalogIn potMeterIn1(pot1name);
+InterruptIn button2(button2name);
+AnalogIn potMeterIn2(pot2name);
 
-DigitalOut led(LED_RED);
+QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING);           //64 = ..., x4 encoding because ...
+QEI Encoder2(D9,D8,NC,64,QEI::X4_ENCODING); 
+// ------- Objects used -------
+ 
+// Ticker which controls the mother every 1/100 of a second.
+Ticker controlTicker;
+ 
+Ticker debugTicker;
+Serial pc(USBTX, USBRX);
+ 
+//HIDScope scope(2); //fuck you hidscope we're not using you!
+ 
+ 
+// ------- Constants
+const float motorGain = 8f;         //just to be sure, niet te veel input geven tegen windup (old motor, suboptimal perfomance)
+const float maxVelocity = 8f; //radians per second
+const bool clockwise = true;
+const float rad_count = 0.0007479;   // 2pi/8400;
+const float pi = 3.1415926;
+ 
+// ------ variables
+ 
+volatile bool direction1 = clockwise;
+volatile bool direction2 = clockwise;
+ 
+// ------ functions
+ 
+float getReferenceVelocity() {
+    // Returns reference velocity in rad/s. 
+    return maxVelocity * potMeterIn1 ;
+    }
+float getReferenceVelocity2() {
+    // Returns reference velocity in rad/s. 
+    return maxVelocity * potMeterIn2;    
+}
+ 
+void setMotor1(float motorValue1) {
+    // Given motorValue1<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor1control.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
+}
+ 
+void setMotor2(float motorValue2) {
+    // Given motorValue2<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor2control.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
+}
+ 
+float feedForwardControl(float referenceVelocity) {
+    // very simple linear feed-forward control
+    // returns motorValue
+    return referenceVelocity / motorGain;
+}
+ 
+float feedForwardControl2(float referenceVelocity2) {
+    // very simple linear feed-forward control
+    // returns motorValue
+    return referenceVelocity2 / motorGain;
+}
+ 
+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 motorValue1 = feedForwardControl(referenceVelocity);
+    setMotor1(motorValue1);
+ 
+    float referenceVelocity2 = getReferenceVelocity2();
+    float motorValue2 = feedForwardControl2(referenceVelocity2);
+    setMotor2(motorValue2);
+} 
+ 
+void onButtonPress() {
+    // reverses the direction
+    motor1direction.write(direction1 = !direction1);
+    pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
+}
+ 
+void onButtonPress2() {
+    // reverses the direction
+    motor2direction.write(direction2 = !direction2);
+    pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
+} 
 
-MODSERIAL pc(USBTX, USBRX);
+float Encoding()
+{
+    // Encoding is necessary for the computations of the joint angles and 
+    // velocities of the arm given certain linear velocities. 
+ 
+    double counts1 = Encoder1.getPulses();
+    double rad_m1 = (rad_count * (double)counts1) + (1.0f*pi/3.0f);
+    return rad_m1;
+} 
+
+float Encoding2()
+{
+    // Encoding is necessary for the computations of the joint angles and 
+    // velocities of the arm given certain linear velocities. 
+ 
+    double counts2 = Encoder2.getPulses();
+    double rad_m2 = (rad_count * (double)counts2) + (7.0f*pi/4.0f);
+    return rad_m2;
+} 
 
+   void onDebugTick() {
+    
+    pc.printf("pot input1: %f\r\n", potMeterIn1.read());
+    pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
+    pc.printf("radialen motor 1: %f\r\n",Encoding());
+
+    pc.printf("\n\n\n");
+    
+    /*
+    scope.set(0, potMeterIn.read());
+    scope.set(1, feedForwardControl(getReferenceVelocity()));
+    scope.send();
+    */
+    pc.printf("pot input2: %f\r\n", potMeterIn2.read());
+    pc.printf("motorValue2: %f\r\n", feedForwardControl2(getReferenceVelocity2()));
+        pc.printf("radialen motor 2: %f\r\n",Encoding2());
+    pc.printf("\n\n\n");
+    }
+    
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
     
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
-    }
-}
+    button2.fall(&onButtonPress2);
+    controlTicker.attach(&measureAndControl, 1.0f/100.0f);
+    debugTicker.attach(&onDebugTick, 1);
+    
+    button1.fall(&onButtonPress);
+    
+}
\ No newline at end of file