Diff: main.cpp

Revision:

0:57aa29917d4c

Child:

1:faa90344cdd8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 01 11:50:51 2016 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "QEI.h"  //bieb voor encoderfuncties in c++
+#include "MODSERIAL.h" //bieb voor modserial
+#include "BiQuad.h"
+
+InterruptIn sw3(SW3);
+DigitalIn sw2(SW2);
+DigitalIn encoder1A(D13);
+DigitalIn encoder1B(D12);
+DigitalIn encoder2A(D11); 
+DigitalIn encoder2B(D10);
+DigitalIn button_cw(D9);
+DigitalIn button_ccw(PTC12);
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut richting_motor1(D4);
+PwmOut pwm_motor1(D5);
+DigitalOut richting_motor2(D7);
+PwmOut pwm_motor2(D6);
+BiQuad PID_controller;
+
+//constanten voor de encoder
+const int CW = 2.5; //definitie rechtsom 'lage waarde'
+const int CCW =-1; //definitie linksom 'hoge waarde'
+const float gearboxratio=131.25; // gearboxratio van encoder naar motor
+const float rev_rond=64.0;        // aantal revoluties per omgang van de encoder
+
+volatile double current_position_motor1 = 0;                // current position is 0 op het begin
+volatile double previous_position_motor1 = 0;               // previous position is 0 op het begin                    
+volatile double current_position_motor2 = 0;                
+volatile double previous_position_motor2 = 0;                                   
+volatile bool tickerflag;                           //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
+volatile double snelheid_motor1;                    // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
+volatile double snelheid_motor2;                    // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
+double ticker_TS=0.025;                              // zorgt voor een tijdstap van de ticker van 0.1 seconde
+volatile double timepassed=0;                       //de waarde van hoeveel tijd er verstreken is
+Ticker t;                                           // maakt ticker t aan
+volatile double value1_resetbutton = 0;             // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien.
+volatile double value2_resetbutton = 0;
+
+
+volatile float d_ref = 0;
+volatile float d_ref_new;
+const float w_ref = 3;
+const double Fs = 0.001;
+const double Kp = 5;
+const double Ki = 5;
+const double Kd = 5; 
+const double N = 100;
+Ticker tick;
+Ticker controllerticker;
+float rev_counts_motor1_rad;
+volatile float voltage_motor1=0.18;
+volatile double error1;
+volatile double ctrlOutput;
+
+void reference(){
+d_ref = d_ref + w_ref * Fs;
+    }
+    
+void m1_controller(){
+    error1 = d_ref-rev_counts_motor1_rad;
+    ctrlOutput = PID_controller.step(error1);
+// if (ctrlOutput > 1){
+//        ctrlOutput =1;
+ //       }
+ //   if (ctrlOutput < 0.01){
+ //       ctrlOutput = 0.01;
+ //       }
+ //   else{
+ //       ctrlOutput = ctrlOutput;
+ //       }
+    }
+
+
+
+int main()
+{
+     pc.baud(115200);  
+QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
+float counts_encoder1;                  //variabele counts aanmaken
+float rev_counts_motor1; 
+
+    
+    while (true) {
+        if (button_cw == 0){
+
+richting_motor1 = 1;
+pwm_motor1 = voltage_motor1;
+counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
+rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); 
+rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
+tick.attach(&reference,Fs);  
+PID_controller.PIDF(Kp,Ki,Kd,N,Fs);
+controllerticker.attach(&m1_controller,Fs);
+voltage_motor1=ctrlOutput;
+pc.printf("%f", voltage_motor1);
+pc.printf("   %f", d_ref);
+pc.printf("   %f \r\n", rev_counts_motor1_rad);
+}
+
+else{
+    pwm_motor1=0;
+    
+}
+    }
+}
\ No newline at end of file