Diff: main.cpp

Revision:

2:ce23bd78a43a

Parent:

1:b71ac293907d

--- a/main.cpp	Thu Apr 08 14:32:30 2021 +0000
+++ b/main.cpp	Thu Apr 15 14:53:13 2021 +0000
@@ -1,16 +1,18 @@
 // Display changes in encoder position and direction 
 #include "mbed.h"
 #include "qeihw.h"
-#define PLAGE_ANGULAIRE 420
-#define GAUCHE_MAX 710
+#define PLAGE_ANGULAIRE 800 // 800 // 420
+#define GAUCHE_MAX 500 // 500 /// 720
+#define TOUT_DROIT 900 
+#define ARRET_MOTEUR_NON_VOULU 3
 #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
+//#define GAUCHE_MAX 710
+//#define PLAGE_ANGULAIRE 420
 
 CAN can2(p30, p29,1000000);
   
@@ -22,12 +24,18 @@
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 
+void print (void);
+
+void serial_ISR(void);
+
 int Bytes2Int(char data[], int position ) ;
 
 void set_position(unsigned short us) ;
 
 void send_can (float vitesse, int nbr) ;
 
+void send_can2(bool Moteur_stopper);
+
 float Bytes2Float( char data[], int position) ;
 
 float Float2Bytes( char data[], float nbr) ;
@@ -37,17 +45,23 @@
 Timer can_timer ;
 Timer mot_timer ;
 Timer capteur_timer ;
+Timer Probleme_arret;
+
+unsigned short pos = 1000;
+float vit = 0 ;
+
 int main() {  
 
-    int32_t Position2, position = 0 , temp  = 0;
+    int32_t Position2, position = 0 , temp  = 0, premier_probleme = 0;
     float vitesse = 1.05 , direction , ValMot, ValServo ;
     unsigned char *chptr;  
-    char Sens = 1; 
+    char Sens = 1, Moteur_stopper = 0; 
+    int ajustement_direction = 0;
     
     CANMessage msg_rx;
     CANMessage msg_tx;
     
-    
+    usb.attach(&serial_ISR,Serial::RxIrq);
        
                    
     qei.SetDigiFilter(10);
@@ -55,17 +69,26 @@
     
     mot_timer.start() ;
     can_timer.start() ;
+    Probleme_arret.start();
     capteur_timer.start() ;
     tempo.start() ;
    
     
     Servo.period_ms(20);
-    pwm_mot.period_ms(20);
+    pwm_mot.period_us(50);
+    inA = 1;   
+    inB = 0; 
     while(1) 
     {  
        if (can2.read(msg_rx)) 
         {
-            ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;;
+            ValServo = (float)msg_rx.data[0]/255.0f* PLAGE_ANGULAIRE + GAUCHE_MAX;
+            if(ValServo>=TOUT_DROIT)
+            {
+                ajustement_direction = (ValServo - TOUT_DROIT)*0.33;
+                ValServo = ValServo - ajustement_direction;
+            }
+   
             ValMot =  float(msg_rx.data[1]/255.0f);
             Sens = msg_rx.data[2] ; 
             inA = Sens;   
@@ -86,16 +109,30 @@
        
         set_position(ValServo);
         pwm_mot.write(ValMot);
-           
+        //set_position(pos);
+        //pwm_mot.write(vit);           
        
             
             
           
-        if(can_timer.read_ms() > CAN_TEMPO ){ 
-           
+        if(can_timer.read_ms() > CAN_TEMPO )
+        { 
+            if (vitesse <= ARRET_MOTEUR_NON_VOULU)
+            {
+                if(premier_probleme == 0)
+                    premier_probleme = Probleme_arret.read_ms();
+                if(Probleme_arret.read_ms() >= premier_probleme + 5000); 
+                    Moteur_stopper = 1;
+                if(vitesse >= ARRET_MOTEUR_NON_VOULU) 
+                    {
+                        Moteur_stopper = 0;
+                        premier_probleme = 0;
+                    }   
+            }
             send_can(vitesse, position) ;
+            send_can2(Moteur_stopper);
             can_timer.reset() ;
-            }
+        }
        
         
           
@@ -134,6 +171,7 @@
     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) ;
@@ -144,15 +182,28 @@
     can2.write(message);
 
 }            
-
-
+void send_can2(bool Moteur_stopper)
+{   
+    CANMessage message2;
+    message2.len=1;
+    message2.data[0] = bool(Moteur_stopper);
+    
+    message2.id=0x500;
+    
+    usb.printf("\rARRET MOTEUR %d",Moteur_stopper);
+    
+    message2.type=CANData;
+    message2.format=CANStandard;
+    can2.write(message2);
+    
+}
 
 
 void set_position(unsigned short us)
 {
     unsigned short period= us+20000;
     float pulse_width;
-    if (tempo.read_ms()> 50)
+    if (tempo.read_ms() > 50)
     {
         pulse_width=((float)us)/((float)period);
         Servo.period_us(period);
@@ -185,3 +236,42 @@
 memcpy(u.bytes, data + position,4) ;
 return u.float_sent ;
 }
+
+void serial_ISR(void){
+    char msg=usb.getc();
+    switch (msg){
+        case 'a':
+            pos=pos+100;
+            break;
+        case 'z':
+            if (pos>100)
+                pos=pos-100;
+            break;
+        case 'e':
+            pos=pos+10;
+            break;
+        case 'r':
+            if (pos>10)
+                pos=pos-10;
+            break;
+        case 'q':
+            if (vit<1.0)
+                vit+=0.1 ;
+            break;
+        case 's':
+            if (vit>0.0)
+                vit-= 0.1 ;
+            break;
+        case 'd':
+            if (vit<1.0)
+                vit+=0.01 ;
+            break;
+        case 'f':
+            if (vit>0.0)
+                vit-=0.01 ;
+            break;
+        
+            
+    }
+    usb.printf("pos = %d us vit = %f \n",pos,vit);
+}