Diff: main.cpp

Revision:

1:b71ac293907d

Parent:

0:a0ab83db7fa0

Child:

2:ce23bd78a43a

--- a/main.cpp	Tue Mar 30 15:45:05 2021 +0000
+++ b/main.cpp	Thu Apr 08 14:32:30 2021 +0000
@@ -4,52 +4,65 @@
 #define PLAGE_ANGULAIRE 420
 #define GAUCHE_MAX 710
 #define BLINKING_RATE_MS 
+#define CAPTEUR_TEMPO 200
+#define CAN_TEMPO 50
+#define MOTEUR_TEMPO 50
 #include "mbed.h"
+#define POS_PER_METER 43636
+#define GAUCHE_MAX 710
+#define PLAGE_ANGULAIRE 420
 
 CAN can2(p30, p29,1000000);
   
 PwmOut Servo(p23);
 PwmOut pwm_mot(p26);
-
+Serial usb(USBTX,USBRX,115200) ;
 DigitalOut inA(p24);
 DigitalOut inB(p25);
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 
-void set_position(unsigned short us);
+int Bytes2Int(char data[], int position ) ;
+
+void set_position(unsigned short us) ;
 
+void send_can (float vitesse, int nbr) ;
 
+float Bytes2Float( char data[], int position) ;
+
+float Float2Bytes( char data[], float nbr) ;
 
 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
 Timer tempo ;
-Timer t;
 Timer can_timer ;
-
+Timer mot_timer ;
+Timer capteur_timer ;
 int main() {  
 
-    can_timer.start() ;
-    tempo.start() ;
-    float vitesse , direction;
+    int32_t Position2, position = 0 , temp  = 0;
+    float vitesse = 1.05 , direction , ValMot, ValServo ;
     unsigned char *chptr;  
-    float ValMot,ValServo;
-    char Sens; 
-    DigitalOut led(LED1);
+    char Sens = 1; 
+    
     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 ;
+    
+    mot_timer.start() ;
+    can_timer.start() ;
+    capteur_timer.start() ;
+    tempo.start() ;
+   
     
     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;;
@@ -60,24 +73,33 @@
                         
         }
        
-        if ( temp%200 == 0 ){   
+        if (capteur_timer.read_ms() > CAPTEUR_TEMPO){   
                 Position2 = qei.GetPosition();  
-                led1 = qei.Direction() == SET ? 1 : 0;
+                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));
+                vitesse = float(Position2 - position)/(POS_PER_METER*0.020)  ;                      
+                //usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r",ValMot, Position2,vitesse);
                 position = Position2 ;
+                capteur_timer.reset() ;
               }  
-              
+        
+       
         set_position(ValServo);
         pwm_mot.write(ValMot);
-        
+           
+       
+            
+            
+          
+        if(can_timer.read_ms() > CAN_TEMPO ){ 
+           
+            send_can(vitesse, position) ;
+            can_timer.reset() ;
+            }
        
         
           
-}
+    }
 }
      
      /******* Régulation PID ********/
@@ -97,7 +119,33 @@
 
 
 
-               
+void send_can (float vitesse, int nbr){
+    float res ;
+    int res2 ;
+    unsigned char *chptr ;
+    CANMessage message;
+    message.len=8;
+    chptr = (unsigned char *) & vitesse;  
+    message.data[0] = (*chptr++);
+    message.data[1] = (*chptr++);
+    message.data[2] = (*chptr++);
+    message.data[3] = (*chptr);
+    message.data[4] = char(nbr) ;
+    message.data[5] = char(nbr>>8) ;
+    message.data[6] = char(nbr>>16) ;
+    message.data[7] = char(nbr>>24) ;
+    message.id=0x400;
+    
+    res = Bytes2Float((char*)message.data, 0) ;
+    res2 = Bytes2Int((char*)message.data, 4) ;
+    usb.printf("\rvitesse = %f,   pos = %d", vitesse, nbr) ;
+    message.type=CANData;
+    message.format=CANStandard;
+    can2.write(message);
+
+}            
+
+
 
 
 void set_position(unsigned short us)
@@ -112,3 +160,28 @@
         tempo.reset();
     }
 }
+
+int Bytes2Int(char data[], int position ){ 
+    int nbr = 0 ;
+    return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
+}  
+       
+
+float Float2Bytes( char data[], float nbr){ 
+    unsigned char* chptr ;
+    chptr = (unsigned char *) & nbr;  
+    data[0]= (*chptr++);
+    data[1]= (*chptr++);
+    data[2]= (*chptr++);
+    data[3]= (*chptr);
+    
+}
+
+float Bytes2Float( char data[], int position){ 
+  union {
+    float float_sent;
+    unsigned char bytes[4];
+  } u;
+memcpy(u.bytes, data + position,4) ;
+return u.float_sent ;
+}