dorian diana / Mbed 2 deprecated Accelero1

ER2ER

Dependencies:   mbed MMA7660

main.cpp@0:62d65a8de2ae, 2021-06-02 (annotated)

Committer:
dorian06
Date:
Wed Jun 02 07:12:38 2021 +0000
Revision:
0:62d65a8de2ae
accelero

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dorian06 0:62d65a8de2ae 1
dorian06 0:62d65a8de2ae 2 /*#include "mbed.h"
dorian06 0:62d65a8de2ae 3 #include "MMA7660.h"
dorian06 0:62d65a8de2ae 4 MMA7660 MMA(p28,p27);
dorian06 0:62d65a8de2ae 5 int main()
dorian06 0:62d65a8de2ae 6 {
dorian06 0:62d65a8de2ae 7 void init();
dorian06 0:62d65a8de2ae 8 float fx,fy,fz;
dorian06 0:62d65a8de2ae 9 printf ("\n\rDebut0");
dorian06 0:62d65a8de2ae 10 while(1) {
dorian06 0:62d65a8de2ae 11 fx=MMA.x();
dorian06 0:62d65a8de2ae 12 fy=MMA.y();
dorian06 0:62d65a8de2ae 13 fz=MMA.z();
dorian06 0:62d65a8de2ae 14 printf ("x=%f,y=%f, z=%f\n\r",fx,fy,fz);
dorian06 0:62d65a8de2ae 15 wait(1);
dorian06 0:62d65a8de2ae 16 }
dorian06 0:62d65a8de2ae 17 }
dorian06 0:62d65a8de2ae 18 */
dorian06 0:62d65a8de2ae 19 #include "mbed.h"
dorian06 0:62d65a8de2ae 20 #include "MMA7660.h"
dorian06 0:62d65a8de2ae 21 // période moteur
dorian06 0:62d65a8de2ae 22 #define PWMPeriod 100
dorian06 0:62d65a8de2ae 23 #define PI 3.14159265358979323846264338327950
dorian06 0:62d65a8de2ae 24 // Moteurs
dorian06 0:62d65a8de2ae 25 PwmOut mot1(p23);
dorian06 0:62d65a8de2ae 26 PwmOut mot2(p22);
dorian06 0:62d65a8de2ae 27 DigitalOut sensG(p12);
dorian06 0:62d65a8de2ae 28 DigitalOut sensD(p11);
dorian06 0:62d65a8de2ae 29 // Boutons
dorian06 0:62d65a8de2ae 30 DigitalIn bpD(p29);
dorian06 0:62d65a8de2ae 31 // Tickers
dorian06 0:62d65a8de2ae 32 Ticker actionChaqueSeconde;
dorian06 0:62d65a8de2ae 33 MMA7660 MMA(p28, p27);
dorian06 0:62d65a8de2ae 34 DigitalOut connectionLed(LED1);
dorian06 0:62d65a8de2ae 35 DigitalOut led1(LED4);
dorian06 0:62d65a8de2ae 36 DigitalOut led2(LED3);
dorian06 0:62d65a8de2ae 37 DigitalOut led3(LED2);
dorian06 0:62d65a8de2ae 38 void calculAngle(void);
dorian06 0:62d65a8de2ae 39 void ChangementVitesse(float, int);
dorian06 0:62d65a8de2ae 40 int etat;
dorian06 0:62d65a8de2ae 41 float angleTot, angle1, angle2, angle3, angle4, angle5, angle6, angle7, angle8;
dorian06 0:62d65a8de2ae 42 int main()
dorian06 0:62d65a8de2ae 43
dorian06 0:62d65a8de2ae 44 {
dorian06 0:62d65a8de2ae 45 mot1.period_us(PWMPeriod);
dorian06 0:62d65a8de2ae 46 mot2.period_us(PWMPeriod);
dorian06 0:62d65a8de2ae 47 mot1.pulsewidth_us(PWMPeriod);
dorian06 0:62d65a8de2ae 48 mot2.pulsewidth_us(PWMPeriod);
dorian06 0:62d65a8de2ae 49 bpD.mode(PullUp);
dorian06 0:62d65a8de2ae 50 ChangementVitesse(0,1);
dorian06 0:62d65a8de2ae 51 ChangementVitesse(0,2);
dorian06 0:62d65a8de2ae 52 actionChaqueSeconde.attach(&calculAngle, 0.02);
dorian06 0:62d65a8de2ae 53 if (MMA.testConnection()){
dorian06 0:62d65a8de2ae 54 connectionLed = 1;
dorian06 0:62d65a8de2ae 55 }
dorian06 0:62d65a8de2ae 56 led1 = led2 = led3 = 0;
dorian06 0:62d65a8de2ae 57 while(1) {
dorian06 0:62d65a8de2ae 58 switch(etat) {
dorian06 0:62d65a8de2ae 59 case 0:
dorian06 0:62d65a8de2ae 60 led1 = led2 = led3 = 0;
dorian06 0:62d65a8de2ae 61 ChangementVitesse(0,1);
dorian06 0:62d65a8de2ae 62 ChangementVitesse(0,2);
dorian06 0:62d65a8de2ae 63 if(bpD.read() == 0)
dorian06 0:62d65a8de2ae 64 etat++;
dorian06 0:62d65a8de2ae 65 break;
dorian06 0:62d65a8de2ae 66 case 1:
dorian06 0:62d65a8de2ae 67 led1 = led2 = led3 = 0;
dorian06 0:62d65a8de2ae 68 led1 = 1;
dorian06 0:62d65a8de2ae 69 ChangementVitesse(48,1);
dorian06 0:62d65a8de2ae 70 ChangementVitesse(50,2);
dorian06 0:62d65a8de2ae 71 if(angleTot > 13) {
dorian06 0:62d65a8de2ae 72 ChangementVitesse(60,1);
dorian06 0:62d65a8de2ae 73 ChangementVitesse(70,2);
dorian06 0:62d65a8de2ae 74 etat++;
dorian06 0:62d65a8de2ae 75 }
dorian06 0:62d65a8de2ae 76 break;
dorian06 0:62d65a8de2ae 77 case 2:
dorian06 0:62d65a8de2ae 78 led1 = led2 = led3 = 0;
dorian06 0:62d65a8de2ae 79 led2 = 1;
dorian06 0:62d65a8de2ae 80 if(angleTot < 7) {
dorian06 0:62d65a8de2ae 81 ChangementVitesse(0,1);
dorian06 0:62d65a8de2ae 82 ChangementVitesse(0,2);
dorian06 0:62d65a8de2ae 83 led1 = led2 = 1;
dorian06 0:62d65a8de2ae 84 wait(3);
dorian06 0:62d65a8de2ae 85 led1 = led2 = led3 = 0;
dorian06 0:62d65a8de2ae 86 led3 = 1;
dorian06 0:62d65a8de2ae 87 ChangementVitesse(45,1);
dorian06 0:62d65a8de2ae 88 ChangementVitesse(50,2);
dorian06 0:62d65a8de2ae 89 wait(2.5);
dorian06 0:62d65a8de2ae 90 etat = 0;
dorian06 0:62d65a8de2ae 91 }
dorian06 0:62d65a8de2ae 92 break;
dorian06 0:62d65a8de2ae 93 }
dorian06 0:62d65a8de2ae 94 }
dorian06 0:62d65a8de2ae 95 }
dorian06 0:62d65a8de2ae 96
dorian06 0:62d65a8de2ae 97 void calculAngle()
dorian06 0:62d65a8de2ae 98 {
dorian06 0:62d65a8de2ae 99 angle8 = angle7;
dorian06 0:62d65a8de2ae 100 angle7 = angle6;
dorian06 0:62d65a8de2ae 101 angle6 = angle5;
dorian06 0:62d65a8de2ae 102 angle5 = angle4;
dorian06 0:62d65a8de2ae 103 angle4 = angle3;
dorian06 0:62d65a8de2ae 104 angle3 = angle2;
dorian06 0:62d65a8de2ae 105 angle2 = angle1;
dorian06 0:62d65a8de2ae 106 angle1 = (1 - MMA.z()) * 90;
dorian06 0:62d65a8de2ae 107 angleTot = (angle1 + angle2 + angle3 + angle4+ angle5 + angle6 + angle7 + angle8) / 8;
dorian06 0:62d65a8de2ae 108 if (angleTot < 0) { // car très rarement négatif
dorian06 0:62d65a8de2ae 109 angleTot *= -1;
dorian06 0:62d65a8de2ae 110 }
dorian06 0:62d65a8de2ae 111 }
dorian06 0:62d65a8de2ae 112
dorian06 0:62d65a8de2ae 113 void ChangementVitesse(float fVitesse, int iMoteur)
dorian06 0:62d65a8de2ae 114 {
dorian06 0:62d65a8de2ae 115 if(fVitesse > 100) {
dorian06 0:62d65a8de2ae 116 fVitesse = 100;
dorian06 0:62d65a8de2ae 117 } else if(fVitesse < -100) {
dorian06 0:62d65a8de2ae 118 fVitesse = -100;
dorian06 0:62d65a8de2ae 119 }
dorian06 0:62d65a8de2ae 120 if(iMoteur == 1) {
dorian06 0:62d65a8de2ae 121 if(fVitesse > 0) { // positive
dorian06 0:62d65a8de2ae 122 mot1.pulsewidth_us(fVitesse);
dorian06 0:62d65a8de2ae 123 sensG.write(0); // avance
dorian06 0:62d65a8de2ae 124 } else {
dorian06 0:62d65a8de2ae 125 mot1.pulsewidth_us(PWMPeriod - (fVitesse * -1));
dorian06 0:62d65a8de2ae 126 sensG.write(1); // recule
dorian06 0:62d65a8de2ae 127 }
dorian06 0:62d65a8de2ae 128 } else if (iMoteur == 2) {
dorian06 0:62d65a8de2ae 129 if(fVitesse > 0) { // positive
dorian06 0:62d65a8de2ae 130 mot2.pulsewidth_us(fVitesse);
dorian06 0:62d65a8de2ae 131 sensD.write(0); // avance
dorian06 0:62d65a8de2ae 132 } else {
dorian06 0:62d65a8de2ae 133 mot2.pulsewidth_us(PWMPeriod - (fVitesse * -1));
dorian06 0:62d65a8de2ae 134 sensD.write(1); // recule
dorian06 0:62d65a8de2ae 135 }
dorian06 0:62d65a8de2ae 136 } else {
dorian06 0:62d65a8de2ae 137 printf("erreur iMoteur incorrect (1 ou 2) dans fonction ChangementVitesse");
dorian06 0:62d65a8de2ae 138 }
dorian06 0:62d65a8de2ae 139
dorian06 0:62d65a8de2ae 140 }