Diff: main.cpp

Revision:

2:5b742cba102c

Parent:

0:b82c05c12d48

--- a/main.cpp	Wed Jun 07 13:27:00 2017 +0000
+++ b/main.cpp	Fri Oct 12 16:30:27 2018 +0000
@@ -1,29 +1,60 @@
 #include "mbed.h"
+#include "QEI.h"
 
 DigitalOut  my_led(LED1);
-InterruptIn my_button(USER_BUTTON);
-PwmOut      my_pwm(PB_3);
+DigitalIn   bouton(USER_BUTTON);
+PwmOut      pwm_moteur1(PA_8);
+DigitalOut  Enable(PB_5);
+PwmOut      pwm_moteur2(PB_3);
+Serial      pc(USBTX,USBRX);
+QEI         codeur(D8, D9, NC, 48, QEI::X4_ENCODING );
+AnalogOut   sortie_position(A2);
+AnalogIn    entree_consigne(A0);
+Ticker      ticker_regul;
+        
+void moteur(float ratio);
+
+void boucle_regul(void){
+    float consigne;
+    float position_roue;
+    static float erreur;
+    static float Ierreur;
+    float Kp = 0.05;
+    float KiTe = 0.00001;
 
-void pressed() {
-    if (my_pwm.read() == 0.25) {
-        my_pwm.write(0.75);
+    Ierreur = Ierreur + erreur;
+    consigne = entree_consigne * 360.0;
+    position_roue = ( ((codeur.getPulses()*10)%9792) * 36.0/(48*20.4) );
+    erreur = consigne - position_roue;
+    moteur(Kp * erreur + KiTe *Ierreur );
+    sortie_position.write(position_roue/360.0);
+    
     }
-    else {
-        my_pwm.write(0.25);
+
+int main()
+{
+    
+    pwm_moteur1.period_us(50);
+    pwm_moteur2.period_us(50);
+    pwm_moteur2.write(0);
+    Enable = 1;
+    ticker_regul.attach(&boucle_regul,0.001);
+    
+    pc.printf("TP asservissement position\n\r");
+    
+    while (1) {
+     
     }
 }
 
-int main()
-{
-    // Set PWM
-    my_pwm.period_ms(10);
-    my_pwm.write(0.5);
-    
-    // Set button
-    my_button.fall(&pressed);
-    
-    while (1) {
-        my_led = !my_led;
-        wait(0.5); // 500 ms
+void moteur(float ratio){
+    if (ratio >= (float)0.0) {
+        pwm_moteur1.write(0);
+        pwm_moteur2.write(ratio);
     }
+    else{
+        pwm_moteur1.write(-ratio);
+        pwm_moteur2.write(0);
+    }       
 }
+        
\ No newline at end of file