CreaLab
Main robot of the 2019 MJCup
Dependencies: LED_WS2812 mbed X_NUCLEO_IHM02A1
Revision 22:82611fe41c5c, committed 2019-09-22
- Comitter:
- alcocerg
- Date:
- Sun Sep 22 06:21:26 2019 +0000
- Parent:
- 21:bf0db5218654
- Commit message:
- First version of the main robot of the 2019 MJCup
Changed in this revision
diff -r bf0db5218654 -r 82611fe41c5c LED_WS2812.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LED_WS2812.lib Sun Sep 22 06:21:26 2019 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/CreaLab/code/LED_WS2812/#15b992a39c77
diff -r bf0db5218654 -r 82611fe41c5c Robot.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h Sun Sep 22 06:21:26 2019 +0000
@@ -0,0 +1,15 @@
+//#include "AsyncServo.h"
+#include "LED_WS2812.h"
+//#include "AsyncBuzzer.h"
+
+// --- USB Debug Port -----------
+
+// #define DEBUG(...) { pc_uart.printf(__VA_ARGS__); bt_uart.printf(__VA_ARGS__);}
+#define DEBUG(...) { __disable_irq();bt_uart.printf(__VA_ARGS__); pc_uart.printf(__VA_ARGS__);__enable_irq();}
+// #define DEBUG(...) { bt_uart.printf(__VA_ARGS__); }
+
+#define CATCH_BUTTON(button, func) button.fall(&func)
+
+#define CASE(letter, text, commands) case letter: if(flaghelp) DEBUG("\t%c : %s\n\r", letter,text);if(!flaghelp) {commands;break;};
+
+
diff -r bf0db5218654 -r 82611fe41c5c main.cpp
--- a/main.cpp Tue Sep 27 13:58:51 2016 +0000
+++ b/main.cpp Sun Sep 22 06:21:26 2019 +0000
@@ -1,53 +1,18 @@
-/**
- ******************************************************************************
- * @file main.cpp
- * @author Davide Aliprandi, STMicroelectronics
- * @version V1.0.0
- * @date November 4th, 2015
- * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM02A1
- * Motor Control Expansion Board: control of 2 motors.
- ******************************************************************************
- * @attention
- *
- * <h2><center>© COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- * 3. Neither the name of STMicroelectronics nor the names of its contributors
- * may be used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ******************************************************************************
+/*
+ * MJBot : version 2019
+ *
*/
-
-
+
/* Includes ------------------------------------------------------------------*/
-/* mbed specific header files. */
#include "mbed.h"
-
-/* Helper header files. */
#include "DevSPI.h"
+#include "x_nucleo_ihm02a1_class.h"
+#undef printf
+#include "Robot.h"
-/* Expansion Board specific header files. */
-#include "x_nucleo_ihm02a1_class.h"
-
+Serial bt_uart(PA_9, PA_10); // PA9 = Tx, PA10 = Rx
+Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3
/* Definitions ---------------------------------------------------------------*/
@@ -55,7 +20,7 @@
#define MPR_1 4
/* Number of steps. */
-#define STEPS_1 (400 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
+#define STEPS_1 (40000 * 128) /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
#define STEPS_2 (STEPS_1 * 2)
/* Delay in milliseconds. */
@@ -63,6 +28,29 @@
#define DELAY_2 2000
#define DELAY_3 5000
+#define MID 1500
+#define MIN 800 // 750
+#define MAX 2200 // 2250
+
+bool receivedCOMMAND;
+char commandRECEIVED;
+int parameterRECEIVED;
+int state;
+char commandLine[256];
+int commandPosition;
+int action;
+int pince;
+int repos;
+int inter;
+int attente;
+int trigger;
+int Bot; // MJBot1 = 0, MJBot2 = 1 and STBot = 2
+
+LED_WS2812 ledBand(PB_10,4);
+PwmOut myservo(PB_4);
+DigitalOut ventilo(PB_0);
+
+// Buzzer buzzer(PB_0);
/* Variables -----------------------------------------------------------------*/
@@ -72,18 +60,18 @@
/* Initialization parameters of the motors connected to the expansion board. */
L6470_Init_t init[L6470DAISYCHAINSIZE] =
{
- /* First Motor. */
+ /* First Motor. */
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the motor. */
- 1.7, /* Max motor phase voltage in A. */
- 3.06, /* Max motor phase voltage in V. */
- 300.0, /* Motor initial speed [step/s]. */
- 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
- 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
- 992.0, /* Motor maximum speed [step/s]. */
+ 12.0, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 0.9, /* Max motor phase voltage in A. */
+ 3.0, /* Max motor phase voltage in V. */
+ 50.0, /* Motor initial speed [step/s]. */
+ 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 200.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
- 602.7, /* Motor full-step speed threshold [step/s]. */
+ 490.0, /* Motor full-step speed threshold [step/s]. */
3.06, /* Holding kval [V]. */
3.06, /* Constant speed kval [V]. */
3.06, /* Acceleration starting kval [V]. */
@@ -96,22 +84,22 @@
3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
- 0xFF, /* Alarm conditions enable. */
+ 0x00, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
},
/* Second Motor. */
{
- 9.0, /* Motor supply voltage in V. */
- 400, /* Min number of steps per revolution for the motor. */
- 1.7, /* Max motor phase voltage in A. */
- 3.06, /* Max motor phase voltage in V. */
- 300.0, /* Motor initial speed [step/s]. */
- 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
- 500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
- 992.0, /* Motor maximum speed [step/s]. */
+ 12, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 0.9, /* Max motor phase voltage in A. */
+ 3.0, /* Max motor phase voltage in V. */
+ 50.0, /* Motor initial speed [step/s]. */
+ 100.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 100.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 200.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
- 602.7, /* Motor full-step speed threshold [step/s]. */
+ 490.0, /* Motor full-step speed threshold [step/s]. */
3.06, /* Holding kval [V]. */
3.06, /* Constant speed kval [V]. */
3.06, /* Acceleration starting kval [V]. */
@@ -124,301 +112,347 @@
3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
- 0xFF, /* Alarm conditions enable. */
+ 0x00, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
}
};
+void help() // Display list of Commands
+{
+ DEBUG("List of commands:\n\r");
+ DEBUG(" h --> Help, display list of commands\n\r");
+}
+
+void executeCommand(char c) {
+ bool flaghelp = false;
+ switch (c) {
+ case 'h':
+ help();
+ action=0;
+ flaghelp=true;
+ CASE('a', "Avance", action=2; )
+ CASE('r', "Recule", action=3; )
+ CASE('d', "Droite AV", action=4; )
+ CASE('g', "Gauche AV", action=5; )
+ CASE('p', "Pivote D", action=6; )
+ CASE('q', "Pivote G", action=7; )
+ CASE('s', "Stop", action=1; )
+ CASE('n', "Debrayer", action=9; )
+ CASE('k', "Pince haute", action=10; )
+ CASE('j', "Pince basse", action=11; )
+ CASE('b', "Ventilo On", action=12; )
+ CASE('l', "Ventilo Off", action=13; )
+ CASE('c', "LEDs ON", action=14; )
+ CASE('x', "LEDs OFF", action=15; )
+ CASE('y', "Pince haute/basse", action=16; )
+ CASE('D', "Droite AR", action=17; )
+ CASE('G', "Gauche AR", action=18; )
+ CASE('t', "Ventilo ON/OFF", action=19; )
+
+ default :
+ DEBUG("invalid command; use: 'h' for help()\n\r");
+ action=0;
+ }}
+
+void analyseCommand(char *command) {
+ switch(command[0]) {
+ case 'a':
+ commandRECEIVED = 'a';
+ break;
+ case 'r':
+ commandRECEIVED = 'r';
+ break;
+ case 'd':
+ commandRECEIVED = 'd';
+ break;
+ case 'g':
+ commandRECEIVED = 'g';
+ break;
+ case 'D':
+ commandRECEIVED = 'D';
+ break;
+ case 'G':
+ commandRECEIVED = 'G';
+ break;
+ case 's':
+ commandRECEIVED = 's';
+ break;
+ case 'p':
+ commandRECEIVED = 'p';
+ break;
+ case 'q':
+ commandRECEIVED = 'q';
+ break;
+ case 'n':
+ commandRECEIVED = 'n';
+ break;
+ case 'k':
+ commandRECEIVED = 'k';
+ break;
+ case 'j':
+ commandRECEIVED = 'j';
+ break;
+ case 'b':
+ commandRECEIVED = 'b';
+ break;
+ case 'l':
+ commandRECEIVED = 'l';
+ break;
+ case 'c':
+ commandRECEIVED = 'c';
+ break;
+ case 'x':
+ commandRECEIVED = 'x';
+ break;
+ case 'y':
+ commandRECEIVED = 'y';
+ break;
+ case 't':
+ commandRECEIVED = 't';
+ break;
+
+ default:
+ commandRECEIVED = 'h';
+ } }
+
+void checkCommand(int result, char *command) {
+ if(result==1) {
+ analyseCommand(command);
+// DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED, parameterRECEIVED,state);
+ receivedCOMMAND = true;
+ } }
+
+void split(char *line, int length) {
+ char command[256];
+ int parameter=0;
+ int result = 1;
+ int i=0;
+ int j=0;
+ while(i<length && line[i]==' ') {
+ i++;}
+ while(i<length && line[i]!=' ') {
+ command[j]=line[i];
+ i++;
+ j++;}
+ command[j]=0;
+ i++;
+ j=0;
+ while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
+ i++;
+ j++;}
+ if(j>0) {
+ result++;
+ i--;
+ int k=1;
+ while(j>0) {
+ parameter += (line[i]-'0')*k;
+ j--;
+ i--;
+ k=k*10;}
+ }
+ checkCommand(result, command);
+}
+
+void storeC(char c) {
+ if(c==10 || c==13|| commandPosition >= 255) {
+ split(commandLine, commandPosition);
+ commandPosition=0;}
+ else {
+ commandLine[commandPosition++]=c;
+ commandLine[commandPosition]=0;}
+}
+
+void getBT() {
+ char c = bt_uart.getc();
+ storeC(c);
+}
+
+void getPC() {
+ char c = pc_uart.getc();
+ storeC(c);
+}
+
+void leda(int parameter) {
+ ledBand.SetColor(WHITE,0) ;
+ ledBand.SetColor(RED,1) ;
+ ledBand.SetColor(BLUE,2) ;
+ ledBand.SetColor(GREEN,3) ;
+ if (parameter == 0) {
+ ledBand.StopRotation();
+// ledBand.StopBlink() ;
+ ledBand.SetColor(BLACK,0) ;
+ ledBand.SetColor(BLACK,1) ;
+ ledBand.SetColor(BLACK,2) ;
+ ledBand.SetColor(BLACK,3) ;}
+ if (parameter == 1) {
+ ledBand.SetColor(RED,0) ;
+ ledBand.SetColor(RED,1) ;
+ ledBand.SetColor(RED,2) ;
+ ledBand.SetColor(RED,3);}
+ if (parameter == 2) {ledBand.StartRotation(0.6);}
+ if (parameter == 3) {ledBand.StartRotation(1);}
+ state=0;
+}
/* Main ----------------------------------------------------------------------*/
int main()
{
- /*----- Initialization. -----*/
+ /*----- Initialization. -----*/
+ wait(1);
+// bt_uart.printf("AT+NAMEPETITBULLES"); // Changement nom module BT
+// wait(2);
/* Initializing SPI bus. */
DevSPI dev_spi(D11, D12, D3);
-
/* Initializing Motor Control Expansion Board. */
x_nucleo_ihm02a1 = new X_NUCLEO_IHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
-
/* Building a list of motor control components. */
L6470 **motors = x_nucleo_ihm02a1->GetComponents();
-
- /* Printing to the console. */
- printf("Motor Control Application Example for 2 Motors\r\n\n");
-
-
- /*----- Setting home and marke positions, getting positions, and going to positions. -----*/
-
- /* Printing to the console. */
- printf("--> Setting home position.\r\n");
-
- /* Setting the home position. */
- motors[0]->SetHome();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- int position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving forward %d steps.\r\n", STEPS_1);
-
- /* Moving. */
- motors[0]->Move(StepperMotor::FWD, STEPS_1);
-
- /* Waiting while active. */
- motors[0]->WaitWhileActive();
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Printing to the console. */
- printf("--> Marking the current position.\r\n");
-
- /* Marking the current position. */
- motors[0]->SetMark();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving backward %d steps.\r\n", STEPS_2);
-
- /* Moving. */
- motors[0]->Move(StepperMotor::BWD, STEPS_2);
-
- /* Waiting while active. */
- motors[0]->WaitWhileActive();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Going to marked position.\r\n");
-
- /* Going to marked position. */
- motors[0]->GoMark();
-
- /* Waiting while active. */
- motors[0]->WaitWhileActive();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Going to home position.\r\n");
-
- /* Going to home position. */
- motors[0]->GoHome();
-
- /* Waiting while active. */
- motors[0]->WaitWhileActive();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Halving the microsteps.\r\n");
-
- /* Halving the microsteps. */
- init[0].step_sel = (init[0].step_sel > 0 ? init[0].step_sel - 1 : init[0].step_sel);
- if (!motors[0]->SetStepMode((StepperMotor::step_mode_t) init[0].step_sel))
- printf(" Step Mode not allowed.\r\n");
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Setting home position.\r\n");
-
- /* Setting the home position. */
- motors[0]->SetHome();
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
- /* Printing to the console. */
- printf("--> Moving forward %d steps.\r\n", STEPS_1);
-
- /* Moving. */
- motors[0]->Move(StepperMotor::FWD, STEPS_1);
-
- /* Waiting while active. */
- motors[0]->WaitWhileActive();
-
- /* Getting the current position. */
- position = motors[0]->GetPosition();
-
- /* Printing to the console. */
- printf("--> Getting the current position: %d\r\n", position);
-
- /* Printing to the console. */
- printf("--> Marking the current position.\r\n");
-
- /* Marking the current position. */
- motors[0]->SetMark();
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- Running together for a certain amount of time. -----*/
-
- /* Printing to the console. */
- printf("--> Running together for %d seconds.\r\n", DELAY_3 / 1000);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareRun(StepperMotor::BWD, 400);
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Waiting. */
- wait_ms(DELAY_3);
-
-
- /*----- Increasing the speed while running. -----*/
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareGetSpeed();
-
- /* Performing the action on each motor at the same time. */
- uint32_t* results = x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Printing to the console. */
- printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
-
- /* Printing to the console. */
- printf("--> Doublig the speed while running again for %d seconds.\r\n", DELAY_3 / 1000);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareRun(StepperMotor::BWD, results[m] << 1);
-
- /* Performing the action on each motor at the same time. */
- results = x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Waiting. */
- wait_ms(DELAY_3);
-
- /* Preparing each motor to perform a run at a specified speed. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareGetSpeed();
-
- /* Performing the action on each motor at the same time. */
- results = x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Printing to the console. */
- printf(" Speed: M1 %d, M2 %d.\r\n", results[0], results[1]);
-
- /* Waiting. */
- wait_ms(DELAY_1);
-
-
- /*----- Hard Stop. -----*/
-
- /* Printing to the console. */
- printf("--> Hard Stop.\r\n");
-
- /* Preparing each motor to perform a hard stop. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareHardStop();
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- Doing a full revolution on each motor, one after the other. -----*/
-
- /* Printing to the console. */
- printf("--> Doing a full revolution on each motor, one after the other.\r\n");
-
- /* Doing a full revolution on each motor, one after the other. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- for (int i = 0; i < MPR_1; i++)
- {
- /* Computing the number of steps. */
- int steps = (int) (((int) init[m].fullstepsperrevolution * pow(2.0f, init[m].step_sel)) / MPR_1);
-
- /* Moving. */
- motors[m]->Move(StepperMotor::FWD, steps);
-
- /* Waiting while active. */
- motors[m]->WaitWhileActive();
-
- /* Waiting. */
- wait_ms(DELAY_1);
- }
-
- /* Waiting. */
- wait_ms(DELAY_2);
-
-
- /*----- High Impedance State. -----*/
-
- /* Printing to the console. */
- printf("--> High Impedance State.\r\n");
-
- /* Preparing each motor to set High Impedance State. */
- for (int m = 0; m < L6470DAISYCHAINSIZE; m++)
- motors[m]->PrepareHardHiZ();
-
- /* Performing the action on each motor at the same time. */
- x_nucleo_ihm02a1->PerformPreparedActions();
-
- /* Waiting. */
- wait_ms(DELAY_2);
-}
\ No newline at end of file
+ pc_uart.printf("MJBot2 \r\n\n"); // Printing to the console
+ commandPosition=0;
+ bt_uart.attach(getBT);
+ pc_uart.attach(getPC);
+ state=0;
+ action=0;
+ pince=0;
+ inter=0;
+ repos=0;
+ attente=0; // temps d'inactivité avant débrayage
+ trigger=0; // est à 1 si Stop envoyé une seule fois
+ Bot=0; // MJBot Tullins = 0, MJBot R2D2 = 1 and STBot = 2
+ ventilo=0;
+ myservo.period_ms(20);
+ myservo.pulsewidth_us(MID);
+ receivedCOMMAND = false;
+ while(1) {
+ if(receivedCOMMAND) {
+ receivedCOMMAND = false;
+ executeCommand(commandRECEIVED);
+ if (action>=1) {
+ if (action==1) {
+ if (repos==1) {
+ pc_uart.printf("stop hard \r\n\n");
+ motors[0]->PrepareHardStop();
+ motors[1]->PrepareHardStop();
+ repos=0;
+ attente=0;
+ trigger=1;
+ inter=0; }
+ else {
+ pc_uart.printf("stop debraye \r\n\n");
+ motors[0]->PrepareSoftHiZ();
+ motors[1]->PrepareSoftHiZ();
+ trigger=0;}
+ x_nucleo_ihm02a1->PerformPreparedActions();}
+ if (action==2) {
+ pc_uart.printf("Avance \r\n\n");
+ motors[0]->PrepareRun(StepperMotor::BWD, 150);
+ motors[1]->PrepareRun(StepperMotor::FWD, 150);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==3) {
+ pc_uart.printf("Recule \r\n\n");
+// motors[0]->Run(StepperMotor::BWD, 200);
+// motors[1]->Run(StepperMotor::FWD, 200);}
+ motors[0]->PrepareRun(StepperMotor::FWD, 150);
+ motors[1]->PrepareRun(StepperMotor::BWD, 150);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==4) {
+ pc_uart.printf("droite AV \r\n\n");
+ // motors[0]->Run(StepperMotor::FWD, 100);
+ motors[0]->PrepareHardStop();
+ motors[1]->PrepareRun(StepperMotor::FWD, 150);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==5) {
+ pc_uart.printf("gauche AV \r\n\n");
+ motors[0]->PrepareRun(StepperMotor::BWD, 150);
+ motors[1]->PrepareHardStop();
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==17) {
+ pc_uart.printf("droite AR \r\n\n");
+ motors[0]->PrepareHardStop();
+ motors[1]->PrepareRun(StepperMotor::BWD, 150);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==18) {
+ pc_uart.printf("gauche AR \r\n\n");
+ motors[0]->PrepareRun(StepperMotor::FWD, 150);
+ motors[1]->PrepareHardStop();
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==6) {
+ pc_uart.printf("pivote d \r\n\n");
+ motors[0]->PrepareRun(StepperMotor::FWD, 75);
+ motors[1]->PrepareRun(StepperMotor::FWD, 75);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+ if (action==7) {
+ pc_uart.printf("pivote g\r\n\n");
+ motors[0]->PrepareRun(StepperMotor::BWD, 75);
+ motors[1]->PrepareRun(StepperMotor::BWD, 75);
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=1; }
+// motors[0]->Run(StepperMotor::FWD, 200);
+// motors[1]->Run(StepperMotor::FWD, 200); }
+ if (action==9) {
+ pc_uart.printf("debrayer \r\n\n");
+ motors[0]->PrepareSoftHiZ();
+ motors[1]->PrepareSoftHiZ();
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ repos=0;
+ trigger=0;
+ attente=0;}
+ if (action==10) {
+ pc_uart.printf("pince haute \r\n\n");
+ myservo.pulsewidth_us(MIN);}
+ if (action==11) {
+ pc_uart.printf("pince basse \r\n\n");
+ myservo.pulsewidth_us(MAX);}
+ if (action==12) {
+ pc_uart.printf("Ventilo On \r\n\n");
+ ventilo=1;
+ }
+ if (action==13) {
+ pc_uart.printf("Ventilo Off \r\n\n");
+ ventilo=0;
+ }
+ if (action==19) {
+ pc_uart.printf("Ventilo ON/OFF \r\n\n");
+ if (ventilo==0) {
+ ventilo=1;}
+ else {ventilo=0;}}
+ if (action==14) {
+ pc_uart.printf("LEDs ON \r\n\n");
+ leda(3);}
+ if (action==15) {
+ pc_uart.printf("LEDs OFF \r\n\n");
+ leda(0);}
+ if (action==16) {
+ pc_uart.printf("pince haute/basse \r\n\n");
+ if (pince==0) {
+ leda(1);
+ inter=0;
+ myservo.pulsewidth_us(MIN);
+ pince=1;}
+ else {
+ myservo.pulsewidth_us(MAX);
+ leda(2);
+ pince=0;}
+
+ action=0;
+ } }
+ wait(0.1);
+ attente++;
+ if (attente>50 && repos==0 && trigger==1) {
+ pc_uart.printf("attente OK\r\n\n");
+ motors[0]->PrepareSoftHiZ();
+ motors[1]->PrepareSoftHiZ();
+ x_nucleo_ihm02a1->PerformPreparedActions();
+ attente=0;
+ trigger=0; }
+ } } }