Diff: main.cpp

Revision:

0:a0ab83db7fa0

Child:

1:b71ac293907d

diff -r 000000000000 -r a0ab83db7fa0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 30 15:45:05 2021 +0000
@@ -0,0 +1,114 @@
+// Display changes in encoder position and direction 
+#include "mbed.h"
+#include "qeihw.h"
+#define PLAGE_ANGULAIRE 420
+#define GAUCHE_MAX 710
+#define BLINKING_RATE_MS 
+#include "mbed.h"
+
+CAN can2(p30, p29,1000000);
+  
+PwmOut Servo(p23);
+PwmOut pwm_mot(p26);
+
+DigitalOut inA(p24);
+DigitalOut inB(p25);
+DigitalOut led1(LED1);
+DigitalOut led3(LED3);
+
+void set_position(unsigned short us);
+
+
+
+QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
+Timer tempo ;
+Timer t;
+Timer can_timer ;
+
+int main() {  
+
+    can_timer.start() ;
+    tempo.start() ;
+    float vitesse , direction;
+    unsigned char *chptr;  
+    float ValMot,ValServo;
+    char Sens; 
+    DigitalOut led(LED1);
+    CANMessage msg_rx;
+    CANMessage msg_tx;
+    
+    int32_t Position2, position , temp  = 0;
+       
+    //serial_port.attach(&serial_ISR,Serial::RxIrq);                 
+    qei.SetDigiFilter(10);
+    qei.SetMaxPosition(0xFFFFFFFF);
+    t.start();  
+    msg_tx.id = 400 ;
+    msg_tx.len = 4 ;
+    
+    Servo.period_ms(20);
+    pwm_mot.period_ms(20);
+    while(1) 
+    {  wait_ms(50) ;
+       if (can2.read(msg_rx)) 
+        {
+            ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;;
+            ValMot =  float(msg_rx.data[1]/255.0f);
+            Sens = msg_rx.data[2] ; 
+            inA = Sens;   
+            inB = !Sens;  
+                        
+        }
+       
+        if ( temp%200 == 0 ){   
+                Position2 = qei.GetPosition();  
+                led1 = qei.Direction() == SET ? 1 : 0;
+                led3 = !led1;
+                vitesse = (Position2 - position);
+                vitesse = (vitesse/42000.0f);
+                vitesse = (vitesse /(200/1000.0f));                            
+                //printf("la pwm est de %f la position est de %lu la vitesse est de %f KM/H\n\r",pwm, Position2,(vitesse*3.6));
+                position = Position2 ;
+              }  
+              
+        set_position(ValServo);
+        pwm_mot.write(ValMot);
+        
+       
+        
+          
+}
+}
+     
+     /******* Régulation PID ********/
+  // Ecart entre la consigne et la mesure
+ // ecart = vref - vitesse;
+ 
+  // Terme proportionnel
+ // P_x = Kp * ecart;
+ 
+  // Calcul de la commande
+//  commande = P_x + I_x;
+ 
+  // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
+  // I_x = I_x + Ki * dt * ecart;
+  /******* Fin régulation PID ********/
+  
+
+
+
+               
+
+
+void set_position(unsigned short us)
+{
+    unsigned short period= us+20000;
+    float pulse_width;
+    if (tempo.read_ms()> 50)
+    {
+        pulse_width=((float)us)/((float)period);
+        Servo.period_us(period);
+        Servo.write(pulse_width);
+        tempo.reset();
+    }
+}