Diff: main.cpp

Revision:

0:8a660654d511

Child:

1:71457a2df34a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 20 10:01:22 2019 +0000
@@ -0,0 +1,77 @@
+#include "mbed.h"
+#include "Encoder.h"
+#include "math.h"
+#include "Phaserunner.h"
+
+Encoder encoder; 
+RawSerial pedalL(PC_10, PC_11, 115200);
+RawSerial pedalR(PC_12, PD_2, 115200); 
+Phaserunner linkspedal(pedalL);
+Phaserunner rechtspedal(pedalR); 
+
+int main(){
+    DigitalOut PedalL_ON_OFF(PC_14);
+    DigitalOut PedalR_ON_OFF(PC_15);
+
+    
+    InterruptIn NullStell(PA_13);  
+    Serial pc(USBTX,USBRX);
+    DigitalOut led(LED2);
+    DigitalOut ON_OFF(PH_1);
+    
+    DigitalOut Abvorne(PB_7);
+    DigitalOut Abhinten(PC_13);
+ 
+    ON_OFF = true;
+
+    uint32_t pulses=0, pulsesOld=0;
+    double angle_rad, angle_grad, angleOld=0;
+    float a=0.0f,b=0.0f,beta=0.0f; // Ellipse Parameters
+    float R=0.0f, phi=0.0f;
+
+    PedalL_ON_OFF = true;
+    PedalR_ON_OFF = true;
+    wait(0.2);
+    PedalL_ON_OFF = false;
+    PedalR_ON_OFF = false;
+    wait(0.2);
+    PedalL_ON_OFF = true;
+    PedalR_ON_OFF = true;
+
+
+    led = false;
+    pc.printf("Hello bike");
+    //NullStell.rise(&GetNull);
+    while(1){
+       
+        //rev = Encoder.getRevolutions();
+        pulses = encoder.read();
+        angle_rad = encoder.readAngle();
+
+        if(pulses != pulsesOld){
+           pc.printf("pulses: %d\n\r",pulses);
+        }
+        if(angle_rad != angleOld){
+            pc.printf("Angle: %.3f\n\r",angle_rad);
+        }
+        phi = angle_rad;
+        // Calcolation of R 
+        float P_Factor = 0.5f;
+        a = 1.0f;
+        b = 0.5f;
+        beta = 0.0f;
+        R=sqrt(pow(a,2) * pow(sin(beta + phi),2) + pow(b,2) * pow(cos(beta + phi),2));
+        
+        linkspedal.setTorque(R*P_Factor);
+        rechtspedal.setTorque(R*P_Factor);
+         pc.printf("Torque: %f\n\r",R*P_Factor);
+        angleOld = angle_rad;
+        pulsesOld = pulses;
+
+        wait_ms(100);
+        
+    }
+    
+    
+    
+}
\ No newline at end of file