Diff: main.cpp

Revision:

12:ab387ced85ab

Parent:

11:9d83bb9a611c

Child:

13:0549a3e57bc4

--- a/main.cpp	Tue May 26 12:04:42 2020 +0000
+++ b/main.cpp	Tue May 26 13:56:30 2020 +0000
@@ -14,14 +14,17 @@
 #define SP 0.5             // pwm moteur en % (0-1)
 
 void calcul(void);
-DigitalOut Led0(LED1);
+
+DigitalOut LedVP(PC_8);  // led Verin P
+DigitalOut LedVM(PC_5);  // led Verin M
+DigitalOut LedOK(PC_6);   // led Système OK
 
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
 
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
 Thread thread (osPriorityAboveNormal);
 EventQueue queue;
-Mutex pr;
+//Mutex pr;
 AnalogIn verin(PC_3);  // lecture pos verin
 AnalogOut ana (PA_5);   // pour debug analogique ISR
 Motor motor(PB_4,PA_1,PA_4,PC_7);
@@ -64,7 +67,8 @@
     DOF.readAccel();
     DOF.readGyro();
 #if viti
-    ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site
+    //ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site USB dessous
+    ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
 #else
     ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
 #endif
@@ -90,21 +94,51 @@
 //
 int main()
 {
+    LedVP=1;
+    LedVM=1;
+    LedOK=1;
     wait(1);
+    LedVP=0;
+    LedVM=0;
+    LedOK=0;
+    wait(1);
+    LedVP=1;
+    LedVM=1;
+    LedOK=1;
     DOF.begin();
     wait(1);
+    LedVP=0;
+    LedVM=0;
+    LedOK=0;
     DOF.calibration();
+    LedVP=1;
+    LedVM=1;
+    LedOK=1;
     wait(1);
+    LedVP=0;
+    LedVM=0;
+    LedOK=0;
     thread.start(callback(&queue,&EventQueue::dispatch_forever));
     tic.attach(queue.event(&calcul),dt);
+    LedOK=1;
     while(1) {
         ana=0.6;
         //moteur
         float x=verin.read();
         float s=0.0;
-        if((angle_final<-ZM)&&(x>FDC_MOINS)) s=SP;
-        else if((angle_final>ZM)&&(x<FDC_PLUS)) s=-SP;
-        else motor.speed(0.0);
+        if((angle_final<-ZM)&&(x>FDC_MOINS)) {
+            s=SP;
+            LedVP=1;
+            LedVM=0;
+        } else if((angle_final>ZM)&&(x<FDC_PLUS)) {
+            s=-SP;
+            LedVP=1;
+            LedVM=0;
+        } else {
+            s=0;
+            LedVP=0;
+            LedVM=0;
+        }
         //
         motor.speed(s);
         //pr.lock();