FRC - Hackathon
Programme de test à priori fonctionnel avec la version 2018 de la carte
Dependencies: PwmIn mbed Encoder_Nucleo_16_bits
Diff: main.cpp
- Revision:
- 6:88b4805d33e1
- Parent:
- 5:4955cb4b3646
- Child:
- 9:2d99e9946b89
--- a/main.cpp Sun Jun 04 15:15:19 2017 +0000
+++ b/main.cpp Wed Jun 07 20:18:43 2017 +0000
@@ -11,14 +11,33 @@
#include "PwmIn.h"
#include "Nucleo_Encoder_16_bits.h"
-#define BOUSSOLE_adress 0xC0
-#define PIXY_adress 0x54
+#define BOUSSOLE_adress 0xC0
+#define PIXY_adress 0x54
+
+#define CC_BLOCSIZE 14
+#define N_BLOCSIZE 12
+
+#define N_BLOCCODE 0xAA55
+#define CC_BLOCCODE 0xAA56
+
-#define CC_BLOCSIZE 14
-#define N_BLOCSIZE 12
+// Comme la nucleo est Little Endian et que l'AS5047D est Big Endian, les codes sont inversés
+// Registre Valeur (BE) Valeur (LE)
+// NOP 0xC000 0x0003
+// ANGLE 0xFFFF 0xFFFF
+// ERROR 0x4001 0x8002
+// SETTING1 (R) 0xC018 0x1803
+// SETTING2 (R) 0x4019 0x8902
+// SETTING1 (W) 0x0018 0x1800
+// SETTING2 (W) 0x8019 0x9801
-#define N_BLOCCODE 0xAA55
-#define CC_BLOCCODE 0xAA56
+#define SPI_READ_NOP 0x0003
+#define SPI_READ_ANGLE 0xFFFF
+#define SPI_READ_ERROR 0x8002
+#define SPI_READ_SETTINGS1 0x1803
+#define SPI_READ_SETTINGS2 0x8902
+#define SPI_WRTIE_SETTINGS1 0x1800
+#define SPI_WRITE_SETTINGS2 0x9801
typedef unsigned char Byte;
typedef unsigned short Word;
@@ -178,9 +197,9 @@
InterruptIn Echo3 (PB_12);
InterruptIn BP (PC_13);
InterruptIn IG (PC_7);
+InterruptIn PWMG (PB_2);
+InterruptIn PWMD (PB_1);
-PwmIn PWMG (PB_2);
-PwmIn PWMD (PB_1);
PwmIn PWMB (PA_15);
PwmOut Pwm_MG (PB_10);
@@ -189,8 +208,8 @@
I2C Bus_I2C (PB_9, PB_8);
-SPI MotG (PC_12, PC_11, PC_10);
-SPI MotD (PB_15, PB_14, PB_13);
+SPI SPIG (PC_12, PC_11, PC_10);
+SPI SPID (PB_15, PB_14, PB_13);
Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7
Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5
@@ -208,11 +227,11 @@
*/
// Structure de temps
-lWord Tick = 0;
+lWord Tick = 0, Gperiod, Dperiod;
// Sémaphore d'interruption
int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
-int FlagTick = 0, FlagTickLed = 0, BPPressed = 0;
+int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
int Pixy_check = -1;
// Dialogue avec la Pixy
@@ -240,6 +259,26 @@
BPPressed = 1;
}
+void PWM_motGRise (void)
+{
+ static lWord oldTime;
+ lWord now;
+ now = times.read_us();
+ Gperiod = now-oldTime;
+ oldTime = now;
+ nbTurnG++;
+}
+
+void PWM_motDRise (void)
+{
+ static lWord oldTime;
+ lWord now;
+ now = times.read_us();
+ Dperiod = now-oldTime;
+ oldTime = now;
+ nbTurnD++;
+}
+
void echo1Rise ()
{
Echo1Start = times.read_us();
@@ -402,8 +441,7 @@
Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
int PIXY_objet;
- double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
- Word SERVO_pos;
+ int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
T_SERVODIR SERVO_dir = S_monte;
char MENU_choix = 0;
@@ -416,40 +454,60 @@
double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
double SD1_dist, SD2_dist, LD1_dist, LD2_dist;
- double MOTG_duty = 0, MOTD_duty = 0;
+ int MOTG_evol = 1, MOTD_evol = 1;
+ double MOTG_duty = 0.5, MOTD_duty = 0.5;
times.reset();
times.start();
+ // Initialisation des interruptions
tick.attach(&tickTime, 0.001);
- Bus_I2C.frequency (100000);
-
- BP.rise (&BPevent);
+ BP.rise (&BPevent);
- Echo1.rise (&echo1Rise);
- Echo2.rise (&echo2Rise);
- Echo3.rise (&echo3Rise);
- Echo1.fall (&echo1Fall);
- Echo2.fall (&echo2Fall);
- Echo3.fall (&echo3Fall);
+ Echo1.rise (&echo1Rise);
+ Echo2.rise (&echo2Rise);
+ Echo3.rise (&echo3Rise);
+ Echo1.fall (&echo1Fall);
+ Echo2.fall (&echo2Fall);
+ Echo3.fall (&echo3Fall);
+
+ PWMG.rise (&PWM_motGRise);
+ PWMD.rise (&PWM_motDRise);
+
+ Pixy.attach (&getPixyByte);
BP.enable_irq();
IG.enable_irq();
Echo1.enable_irq();
Echo2.enable_irq();
Echo3.enable_irq();
+ PWMG.enable_irq();
+ PWMD.enable_irq();
- Pixy.attach (&getPixyByte);
+ // Initialisation des périphériques
+ // Bus I2C
+ Bus_I2C.frequency (100000);
+ // PWM des moteurs
Pwm_MG.period_us(50);
Pwm_MD.period_us(50);
-
En = 0;
+ // Bus SPI
+ SPIG.format (16,1);
+ SPIG.frequency (1000000);
+
+ SPID.format (16,1);
+ SPID.frequency (1000000);
+
+ SS = 1;
+
+ // Led
Led2 = 0;
- Servo.period_ms(20);
+ Servo.period_ms (20);
+ Servo.pulsewidth_us(200);
while(1) {
@@ -460,11 +518,11 @@
Pc.printf ("\r2- Boussole et I2C\n");
Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
- Pc.printf ("\r5- VBAT\n");
- Pc.printf ("\r6- Moteur Gauche -- NON CODE --\n");
- Pc.printf ("\r7- Moteur Droit -- NON CODE --\n");
+ Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n");
+ Pc.printf ("\r6- Moteur Gauche\n");
+ Pc.printf ("\r7- Moteur Droit\n");
Pc.printf ("\r8- Servomoteur\n");
- Pc.printf ("\r9- PIXY (CMUCAM5)\n\r");
+ Pc.printf ("\r9- PIXY (CMUCAM5)\n");
MENU_choix = Pc.getc ();
} while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));
@@ -657,33 +715,50 @@
En = 1;
SensG = 1;
+ Led2 = 1;
MOTG_duty = 0.5;
+ MOTG_evol = 1;
Pwm_MG = 0.5;
-
+
do {
if (FlagTickLed) {
Led1 = !Led1;
FlagTickLed = 0;
}
-
if (BPPressed) {
BPPressed = 0;
+ SensG = !SensG;
+ Led2 = !Led2;
+ }
- if (!SensG) {
- SensG = 1;
+ if (((Tick%1000)==0) && FlagTick) {
+ FlagTick = 0;
+ if (MOTG_evol) {
+ if (MOTG_duty < 0.9) MOTG_duty += 0.1;
+ else {
+ MOTG_evol = 0;
+ MOTG_duty = 0.9;
+ }
} else {
- SensG = 0;
+ if (MOTG_duty > 0.1) MOTG_duty -= 0.1;
+ else {
+ MOTG_evol = 1;
+ MOTG_duty = 0.1;
+ }
}
- Pwm_MG = MOTG_duty;
}
-
- Pc.printf ("\rPos = %ld Tick", Gauche.GetCounter());
-
+
+ Pwm_MG = MOTG_duty;
+ Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod);
+
+
} while(!Pc.readable());
MENU_choix = Pc.getc();
+
En = 0;
if (SensG == 0) Pwm_MG = 0;
else Pwm_MG = 1;
+
break;
case 7 :
@@ -692,7 +767,9 @@
En = 1;
SensD = 1;
+ Led2 = 1;
MOTD_duty = 0.5;
+ MOTD_evol = 1;
Pwm_MD = 0.5;
do {
@@ -702,26 +779,42 @@
}
if (BPPressed) {
BPPressed = 0;
- if (SensD) {
- SensD = 0;
+ SensD = !SensD;
+ Led2 = !Led2;
+ }
+
+ if (((Tick%1000)==0) && FlagTick) {
+ FlagTick = 0;
+ if (MOTD_evol) {
+ if (MOTD_duty < 0.9) MOTD_duty += 0.1;
+ else {
+ MOTD_evol = 0;
+ MOTD_duty = 0.9;
+ }
} else {
- SensD = 1;
+ if (MOTD_duty > 0.1) MOTD_duty -= 0.1;
+ else {
+ MOTD_evol = 1;
+ MOTD_duty = 0.1;
+ }
}
- Pwm_MD = MOTD_duty;
}
-
- Pc.printf ("\rPos = %ld Tick", Droite.GetCounter());
-
+ Pwm_MD = MOTD_duty;
+ Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod);
+
} while(!Pc.readable());
MENU_choix = Pc.getc();
+
En = 0;
if (SensD == 0) Pwm_MD = 0;
else Pwm_MD = 1;
+
break;
case 8 :
Pc.printf ("\n\n\rServo Moteur\n");
Pc.printf ("\rAppuyez sur Entree pour quitter\n");
+
do {
if (FlagTickLed) {
Led1 = !Led1;
@@ -729,25 +822,25 @@
}
if (((Tick%250)==0) && FlagTick) {
+ FlagTick = 0;
if (SERVO_dir == S_monte) {
- if (SERVO_angle >= SERVO_angleMAX) {
+ if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100;
+ else {
SERVO_dir = S_descente;
- SERVO_angle = SERVO_angleMAX;
- } else {
- SERVO_angle += 5;
+ SERVO_pulseWidth = SERVO_max;
}
} else {
- if (SERVO_angle <= SERVO_angleMIN) {
+ if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100;
+ else {
SERVO_dir = S_monte;
- SERVO_angle = SERVO_angleMIN;
- } else {
- SERVO_angle -= 5;
+ SERVO_pulseWidth = SERVO_min;
}
}
- SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000;
- Servo.pulsewidth_us(SERVO_pos);
- Pc.printf ("\rAngle = %4.1lf", SERVO_angle);
+
+ Servo.pulsewidth_us (SERVO_pulseWidth);
+ Pc.printf ("\rPulse = %d",SERVO_pulseWidth);
}
+
} while(!Pc.readable());
MENU_choix = Pc.getc();
break;