Diff: main.cpp

Revision:

0:62d65a8de2ae

diff -r 000000000000 -r 62d65a8de2ae main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 02 07:12:38 2021 +0000
@@ -0,0 +1,140 @@
+
+/*#include "mbed.h"
+#include "MMA7660.h"
+MMA7660 MMA(p28,p27);
+int main()
+{
+    void init();
+    float fx,fy,fz;
+    printf ("\n\rDebut0");
+    while(1) {
+        fx=MMA.x();
+        fy=MMA.y();
+        fz=MMA.z();
+        printf ("x=%f,y=%f, z=%f\n\r",fx,fy,fz);
+        wait(1);
+    }
+}
+*/
+#include "mbed.h"
+#include "MMA7660.h"
+// période moteur
+#define PWMPeriod 100
+#define PI 3.14159265358979323846264338327950
+// Moteurs
+PwmOut mot1(p23);
+PwmOut mot2(p22);
+DigitalOut sensG(p12);
+DigitalOut sensD(p11);
+// Boutons
+DigitalIn bpD(p29);
+// Tickers
+Ticker actionChaqueSeconde;
+MMA7660 MMA(p28, p27);
+DigitalOut connectionLed(LED1);
+DigitalOut led1(LED4);
+DigitalOut led2(LED3);
+DigitalOut led3(LED2);
+void calculAngle(void);
+void ChangementVitesse(float, int);
+int etat;
+float angleTot, angle1, angle2, angle3, angle4, angle5, angle6, angle7, angle8;
+int main()
+
+{
+    mot1.period_us(PWMPeriod);
+    mot2.period_us(PWMPeriod);
+    mot1.pulsewidth_us(PWMPeriod);
+    mot2.pulsewidth_us(PWMPeriod);
+    bpD.mode(PullUp);
+    ChangementVitesse(0,1);
+    ChangementVitesse(0,2);
+    actionChaqueSeconde.attach(&calculAngle, 0.02);
+    if (MMA.testConnection()){
+        connectionLed = 1;
+        }
+    led1 = led2 = led3 = 0;
+    while(1) {
+        switch(etat) {
+            case 0:
+                led1 = led2 = led3 = 0;
+                ChangementVitesse(0,1);
+                ChangementVitesse(0,2);
+                if(bpD.read() == 0)
+                    etat++;
+                break;
+            case 1:
+                led1 = led2 = led3 = 0;
+                led1 = 1;
+                ChangementVitesse(48,1);
+                ChangementVitesse(50,2);
+                if(angleTot >  13) {
+                    ChangementVitesse(60,1);
+                    ChangementVitesse(70,2);
+                    etat++;
+                }
+                break;
+            case 2:
+                led1 = led2 = led3 = 0;
+                led2 = 1;
+                if(angleTot <  7) {
+                    ChangementVitesse(0,1);
+                    ChangementVitesse(0,2);
+                    led1 = led2 = 1;
+                    wait(3);
+                    led1 = led2 = led3 = 0;
+                    led3 = 1;
+                    ChangementVitesse(45,1);
+                    ChangementVitesse(50,2);
+                    wait(2.5);
+                    etat = 0;
+                }
+                break;
+        }
+    }
+}
+
+void calculAngle()
+{
+    angle8 = angle7;
+    angle7 = angle6;
+    angle6 = angle5;
+    angle5 = angle4;
+    angle4 = angle3;
+    angle3 = angle2;
+    angle2 = angle1;
+    angle1 = (1 - MMA.z()) * 90;
+    angleTot = (angle1 + angle2 + angle3 + angle4+ angle5 + angle6 + angle7 + angle8) / 8;
+    if (angleTot < 0) { // car très rarement négatif
+        angleTot *= -1;
+    }
+}
+
+void ChangementVitesse(float fVitesse, int iMoteur)
+{
+    if(fVitesse > 100) {
+        fVitesse = 100;
+    } else if(fVitesse < -100) {
+        fVitesse = -100;
+    }
+    if(iMoteur == 1) {
+        if(fVitesse > 0) { // positive
+            mot1.pulsewidth_us(fVitesse);
+            sensG.write(0); // avance
+        } else {
+            mot1.pulsewidth_us(PWMPeriod - (fVitesse * -1));
+            sensG.write(1); // recule
+        }
+    } else if (iMoteur == 2) {
+        if(fVitesse > 0) { // positive
+            mot2.pulsewidth_us(fVitesse);
+            sensD.write(0); // avance
+        } else {
+            mot2.pulsewidth_us(PWMPeriod - (fVitesse * -1));
+            sensD.write(1); // recule
+        }
+    } else {
+        printf("erreur iMoteur incorrect (1 ou 2) dans fonction ChangementVitesse");
+    }
+
+}
\ No newline at end of file