Code : Tout sélectionner
#include <AccelStepper.h>
#include <AsyncSonarLib.h>
#include <OneButton.h>
#define LIGHT_VEILLE 5
#define LIGHT_VEILLE_BLINK 10
#define LIGHT_ACTIVATION 255
#define LIGHT_VEILLE2 80
#define LIGHT_RAZ 80
#define MOTOR_POS_VEILLE 400
#define MOTOR_REF_0 0
#define MOTOR_REF_MAX 1000
#define DELAY_RAZ 20000
#define DELAY_LIGHT_VEILLE 5000
#define DELAY_LIGHT_VEILLE2 5000
#define DELAY_LIGHT_ACTIVATION 5000
#define DELAY_GO_VEILLE2 20000
#define DELTA_MOVE_MIN 50
#define DELTA_MOVE_MAX 300
#define DEBUG 1
#define WITH_MOTOR 0
// level de log debug
#define LOG_DEBUG 1
#define PIN_LAMP 11
#define PIN_STEP 8
#define PIN_DIR 4
#define PIN_UP A0
#define PIN_DOWN A1
#define PIN_SONAR A5
#define BANDGAP 10
#define MAXSPEED_WORKMDODE 500
#define MAXSPEED_TUNINGMODE 100
struct slope
{
uint32_t startTime;
uint8_t startValue; // (0-255)
uint8_t stopValue; // (255-0)
uint16_t slopeDuration; // (1-65535)
int32_t currentValue;
};
struct variablesToSave
{
uint16_t stepToFloor;
byte dummy_1; // just for note
int dummy_2 = -8; // just for note
};
enum enumMotorState
{
INIT,
OFF,
UP,
DOWN,
};
enumMotorState motorState, previousMotorState;
boolean onEntryState;
int8_t sens;
slope lampSlope;
#ifdef DEBUG
uint32_t u32_cpu_load;
#endif
uint16_t toto, oldtoto, initMeasureRAZ, lastMeasureSensor;
unsigned long myTime, lastTimeStep;
variablesToSave eeprom;
enum enumOeuvreState
{
VEILLE,
ACTIVATION, // dès qu'on met la main
VEILLE2,
RAZ // on recherche la position 0
};
enumOeuvreState oeuvreState, previousOeuvreState;
enum enumScenarState
{
INIT_SCENAR,
RUN
};
enumScenarState scenarState;
enum enumSONARState
{
INIT_SONAR,
STABLE
};
struct scenar
{
enumOeuvreState oeuvreState;
enumScenarState scenarState;
unsigned long timeValue; // valeur de temps utilisable pour contrôler ce que l'on veut
long positionMotor; // position du moteur demandée, on ne change pas tant qu'on n'y est pas
enumMotorState motorState;
};
scenar currentScenar;
enumSONARState sonarState;
unsigned long lastTSMesure=0;
/***** Prototype definition of code *****/
void slopeInit(slope *thisSlope, uint16_t startValue, uint16_t stopValue, uint16_t slopeDuration);
boolean slopeUpdate(slope *thisSlope);
void PingRecieved(AsyncSonar &sonar);
void TimeOut(AsyncSonar &sonar);
unsigned int getSonar();
void changeLight(int value);
void changeMotor(long value);
void scenarVeille();
void scenarActivation();
void scenarVeille2();
void scenarRAZ();
/***** Beginning of code *****/
AccelStepper stepper(AccelStepper::DRIVER, PIN_STEP, PIN_DIR);
//AccelStepper stepper(AccelStepper::FULL4WIRE, 2, 3, 5, 6);
AsyncSonar sonar(PIN_SONAR, PingRecieved, TimeOut);
OneButton buttonUp(PIN_UP, true, true); // active low, pull-up
OneButton buttonDown(PIN_DOWN, true, true); // active low, pull-up
int currentPosition_light;
long currentPosition_motor;
boolean initStart;
unsigned int absTmp(int v){
return (v>=0)?v:(-1*v);
}
void setup()
{
#ifdef DEBUG
Serial.begin(250000); // for debug purposes
#endif
// initialize lamp
pinMode(PIN_LAMP, OUTPUT);
analogWrite(PIN_LAMP, 0);
// buttonUp.attachClick(clickbuttonUp);
// buttonUp.attachDoubleClick(doubleclickbuttonUp);
// buttonUp.attachLongPressStart(longPressStartbuttonUp);
// buttonUp.attachLongPressStop(longPressStopbuttonUp);
// buttonUp.attachDuringLongPress(longPressbuttonUp);
// // link the button 2 functions.
// buttonDown.attachClick(clickButtonDown);
// buttonDown.attachDoubleClick(doubleclickButtonDown);
// buttonDown.attachLongPressStart(longPressStartButtonDown);
// buttonDown.attachLongPressStop(longPressStopButtonDown);
// buttonDown.attachDuringLongPress(longPressButtonDown);
// initialize stepper;
stepper.setMaxSpeed(MAXSPEED_WORKMDODE);
stepper.setAcceleration(500);
// initialize sonar
sonar.Start(500);
sonarState = INIT_SONAR;
// initialize motor state machine
//motorState = INIT;
//previousMotorState = ~INIT;
// oeuvreState = RAZ;
// myTime = millis();
// changeScenar(RAZ);
//lastTSMesure=myTime;
initStart=true;
// previousOeuvreState =
// slopeInit(&lampSlope, 0, 10, 3000);
}
void loop()
{
myTime = millis();
if(lastTSMesure==myTime||initStart==true){
initStart=false;
Serial.println("TEST LUMIERE à 0 seconde");
analogWrite(PIN_LAMP, LIGHT_VEILLE);
Serial.println("TEST moteur move(-10)");
//stepper.move(-10);
stepper.move(100);
Serial.print("sonar:");
Serial.println(getSonar());
}
if(myTime==(lastTSMesure+5000)){
Serial.println("TEST LUMIERE à 5 secondes");
analogWrite(PIN_LAMP, LIGHT_VEILLE2);
Serial.println("TEST Moteur move(10)");
stepper.move(500);
Serial.print("sonar:");
Serial.println(getSonar());
}
if(myTime==(lastTSMesure+10000)){
Serial.println("TEST LUMIERE à 10 secondes");
analogWrite(PIN_LAMP, 0);
Serial.println("TEST Moteur moveTo(0)");
stepper.moveTo(0);
Serial.print("sonar:");
Serial.println(getSonar());
}
if(myTime==(lastTSMesure+15000)){
Serial.println("Reinit myTime à 15s");
lastTSMesure=myTime+10;
}
sonar.Update(&sonar);
stepper.run();
delay(1);
}
// ----- button Up callback functions
// This function will be called when the button1 was pressed 1 time (and no 2. button press followed).
void clickbuttonUp()
{
stepper.move(10);
// analogWrite(PIN_LAMP, LIGHT_VEILLE);
// if(oeuvreState == VEILLE){
// analogWrite(PIN_LAMP, LIGHT_VEILLE);
// oeuvreState = ACTIVATION;
// Serial.println("VEILLE");
// }else if(oeuvreState == ACTIVATION){
// analogWrite(PIN_LAMP, LIGHT_ACTIVATION);
// oeuvreState = VEILLE2;
// Serial.println("ACTIVATION");
// }else if(oeuvreState == VEILLE2){
// analogWrite(PIN_LAMP, LIGHT_VEILLE2);
// oeuvreState = RAZ;
// Serial.println("VEILLE2");
// }else if(oeuvreState == RAZ){
// analogWrite(PIN_LAMP, 0);
// oeuvreState = VEILLE;
// Serial.println("RAZ");
// }
} // clickUp
// This function will be called when the button1 was pressed 2 times in a short timeframe.
void doubleclickbuttonUp()
{
// EEPROM.put(0, eeprom);
Serial.println("Button up saveEEPROM.");
} // doubleclickbuttonUp
// This function will be called once, when the button1 is pressed for a long time.
void longPressStartbuttonUp()
{
Serial.println("Button up longPress start");
} // longPressStart1
// This function will be called often, while the button1 is pressed for a long time.
void longPressbuttonUp()
{
stepper.move(10);
} // longPressbuttonUp
// This function will be called once, when the button1 is released after beeing pressed for a long time.
void longPressStopbuttonUp()
{
Serial.println("Button up longPress stop");
} // longPressStopbuttonUp
// ... and the same for button Down:
void clickButtonDown()
{
stepper.move(-10);
// if(oeuvreState == VEILLE){
// //analogWrite(PIN_LAMP, LIGHT_VEILLE);
// changeLight(LIGHT_VEILLE)
// oeuvreState = ACTIVATION;
// Serial.println("VEILLE");
// }else if(oeuvreState == ACTIVATION){
// analogWrite(PIN_LAMP, LIGHT_ACTIVATION);
// oeuvreState = VEILLE2;
// Serial.println("ACTIVATION");
// }else if(oeuvreState == VEILLE2){
// analogWrite(PIN_LAMP, LIGHT_VEILLE2);
// oeuvreState = RAZ;
// Serial.println("VEILLE2");
// }else if(oeuvreState == RAZ){
// analogWrite(PIN_LAMP, 0);
// oeuvreState = VEILLE;
// Serial.println("RAZ");
// }
} // clickButtonDown
/**
* This function is called when the button is pressed.
*
* It saves the current state of the EEPROM to the EEPROM.
*
* It then prints a message to the serial monitor
*/
void doubleclickButtonDown()
{
// EEPROM.put(0, eeprom);
Serial.println("Button Down saveEEPROM.");
} // doubleclickButtonDown
void longPressStartButtonDown()
{
Serial.println("Button down longPress start");
} // longPressStartButtonDown
/**
* Move the stepper motor 10 steps backward.
*/
void longPressButtonDown()
{
stepper.move(-10);
} // longPressButtonDown
void longPressStopButtonDown()
{
Serial.println("Button down longPress stop");
} // longPressStopButtonDown
// slopeInit(&mySlope, maxValue, 0, 3000);
void slopeInit(slope *thisSlope, uint16_t startValue, uint16_t stopValue, uint16_t slopeDuration)
{
thisSlope->startTime = millis();
thisSlope->startValue = startValue;
thisSlope->stopValue = stopValue;
thisSlope->slopeDuration = slopeDuration;
thisSlope->currentValue = startValue;
}
// slopeUpdate(&mySlope);
/**
* If the current value is not equal to the stop value, then the current value is calculated by
* multiplying the time elapsed by the slope duration, and then dividing that by the slope duration.
* The current value is then added to the start value
*
* @param thisSlope the slope object to update
*
* @return A boolean value.
*/
boolean slopeUpdate(slope *thisSlope)
{
if (thisSlope->currentValue != thisSlope->stopValue)
{
// if((millis() - thisSlope->startTime) <= thisSlope->slopeDuration) {
thisSlope->currentValue = ((millis() - thisSlope->startTime) * (thisSlope->stopValue - thisSlope->startValue));
thisSlope->currentValue /= thisSlope->slopeDuration;
thisSlope->currentValue += thisSlope->startValue;
return (false);
}
else
{
return (true);
}
}
// ping callback
void PingRecieved(AsyncSonar &sonar2)
{
//Serial.print("Ping: ");
//Serial.println(sonar2.GetMeasureMM());
sonarState = STABLE;
}
// timeout callback
void TimeOut(AsyncSonar &sonar2)
{
sonarState = INIT_SONAR;
//Serial.println("TimeOut");
}
// enumOeuvreState oeuvreState;
// enumScenarState scenarState;
// unsigned long timeValue; // valeur de temps utilisable pour contrôler ce que l'on veut
/**
* It changes the state of the current scenar.
*
* @param oeuvreState the state of the oeuvre (INIT, PLAY, PAUSE, STOP)
*/
void changeScenar(enumOeuvreState oeuvreState)
{
currentScenar.oeuvreState = oeuvreState;
currentScenar.scenarState = INIT_SCENAR;
currentScenar.timeValue = myTime;
currentScenar.positionMotor = 0;
currentScenar.motorState=INIT;
Serial.println("changeScenar: ");
switch (currentScenar.oeuvreState)
{
case VEILLE:
Serial.println("VEILLE");
break;
case ACTIVATION:
Serial.println("ACTIVATION");
break;
case VEILLE2:
Serial.println("VEILLE2");
break;
case RAZ:
Serial.println("RAZ");
break;
default:
break;
}
}
/**
* Change the light to the value given in parameter.
*
* @param value The value to set the pin to.
*/
void changeLight(int value)
{
// si la valeur est la même que l'actuelle on ne fait rien
if (value != currentPosition_light)
{
analogWrite(PIN_LAMP, value);
currentPosition_light = value;
#ifdef LOG_DEBUG
Serial.print("changeLight: ");
Serial.println(currentPosition_light);
#endif
}
}
/**
* If the value is different from the current position, move the motor to the new position
*
* @param value the value of the input
*/
void changeMotor(long value)
{
if(value < MOTOR_REF_0)value=MOTOR_REF_0;
if(value > MOTOR_REF_MAX)value=MOTOR_REF_MAX;
// si la valeur est la même que l'actuelle on ne fait rien
if (value != currentPosition_motor)
{
#ifdef WITH_MOTOR
stepper.moveTo(value);
#endif
if(value>currentPosition_motor)currentScenar.motorState=UP;
else currentScenar.motorState=DOWN;
currentPosition_motor = value;
currentScenar.positionMotor = currentPosition_motor;
// INIT,
// OFF,
// UP,
// DOWN,
#ifdef LOG_DEBUG
Serial.print("changeMotor: ");
Serial.println(currentPosition_motor);
#endif
}else{
currentScenar.motorState=OFF;
}
}
// Vérifie si le moteur est arrivé à destination ou bien?
enumMotorState updateStateMotor(){
if (currentScenar.positionMotor == stepper.currentPosition()){
currentScenar.motorState=OFF;
}
return currentScenar.motorState;
}
// TODO : permettre d'indiquer si la valeur est correcte ou pas?
unsigned int getSonar(){
// on retourne
return sonar.GetMeasureMM();
}
/**
* If the time is greater than or equal to the delayStop, return true
*
* @param timeStart the time at which the timer starts
* @param delayStop The time in milliseconds that you want to delay the program.
*
* @return A boolean value.
*/
boolean diffTime(unsigned long timeStart, unsigned long delayStop){
if (myTime - timeStart >= delayStop){
return true;
}
return false;
}
/**
* Scenario de veille déclenché après un temps non négligeable 30 secondes
*/
void scenarVeille()
{
if (currentScenar.scenarState == INIT_SCENAR)
{
// on accèlère le mouvement
stepper.setMaxSpeed(1000);
stepper.setAcceleration(500);
changeMotor(MOTOR_POS_VEILLE);
// On remet la lumière si besoin
slopeInit(&lampSlope, currentPosition_light, LIGHT_VEILLE, DELAY_LIGHT_VEILLE);
initMeasureRAZ=getSonar();
currentScenar.scenarState=RUN;
}
else
{
if(!slopeUpdate(&lampSlope)){
changeLight(int(lampSlope.currentValue));
}else
{
//Effet clignotement
// on peut fare varier la lumière ici
int lightTmp=int(lampSlope.currentValue)==LIGHT_VEILLE?LIGHT_VEILLE_BLINK:LIGHT_VEILLE;
slopeInit(&lampSlope, currentPosition_light, lightTmp, DELAY_LIGHT_VEILLE);
}
if (sonarState == STABLE)
{
// si le sonar est coupé par qq1, on passe en Activation
// et que la position du moteur est là où on l'a demandé
if(getSonar() < 1500 && updateStateMotor() == OFF){
changeScenar(ACTIVATION);
}
}
}
// uint16_t mesureTmp = sonar.GetMeasureMM();
// // lumière LIGHT_VEILLE
// if (mesureTmp < initMeasureRAZ && mesureTmp > 1500 && initMeasureRAZ - mesureTmp > 100)
// {
// // on passe en activation
// changeLight(LIGHT_ACTIVATION);
// oeuvreState = ACTIVATION;
// lastMeasureSensor = mesureTmp;
// stepper.moveTo(0);
// Serial.println("oeuvreState = ACTIVATION;");
// }
// else
// {
// changeLight(LIGHT_VEILLE);
// }
// position POSITION_VEILLE
// lumière LIGHT_VEILLE
}
/**
* Scenario d'activation
*/
unsigned long lastMoves=0; // dernier mouveent enregistrés que ça soit le moteur qui bouge ou le sonar
void scenarActivation()
{
// on allume la lumière à la valeur max
// on fait bouger selon le sonar
// Si rien pendant 30S on va en veille2
if (currentScenar.scenarState == INIT_SCENAR)
{
// changeMotor(MOTOR_POS_VEILLE);
// On remet la lumière si besoin
slopeInit(&lampSlope, currentPosition_light, LIGHT_ACTIVATION, DELAY_LIGHT_ACTIVATION);
lastMeasureSensor = getSonar();
currentScenar.scenarState = RUN;
}
else
{
if (!slopeUpdate(&lampSlope))
{
lastMoves = myTime;
changeLight(int(lampSlope.currentValue));
}
else
{
// si le moteur n'est pas en déplacement : càd dire qu'il a atteint sa position
if (updateStateMotor() == OFF)
{
if (sonarState == STABLE)
{
uint16_t mesureTmp = getSonar();
int diffMesure = lastMeasureSensor - mesureTmp;
// FIXME : gérer une fonction ABS
int absDiffMesure = absTmp(diffMesure);
// FIXME : régler les valeurs MIN et MAX
if (absDiffMesure > DELTA_MOVE_MIN && diffMesure < DELTA_MOVE_MAX)
{
changeMotor(currentScenar.positionMotor + diffMesure);
lastMoves = myTime;
#ifdef LOG_DEBUG
Serial.print("diffMesure > 50 < 500 : ");
Serial.println(diffMesure);
#endif
}
else
{
#ifdef LOG_DEBUG
Serial.print("diffMesure < 50 || > 500 : ");
Serial.println(diffMesure);
#endif
}
}
}
else
{
// on bouge donc on met à jour le lastMoves
lastMoves = myTime;
}
}
// si pas de modif du sonar et pas d'action depuis
if(myTime-lastMoves>DELAY_GO_VEILLE2){
changeScenar(VEILLE2);
}
// // si le sonar est coupé, on passe en Activation
// if (getSonar() < 1500)
// {
// changeScenar(ACTIVATION);
// }
}
}
void scenarVeille2()
{
// on allume la lumière à la valeur veille2
// et si on bouge devant le capteur on revient
if (currentScenar.scenarState == INIT_SCENAR)
{
// On remet la lumière si besoin
slopeInit(&lampSlope, currentPosition_light, LIGHT_VEILLE2, DELAY_LIGHT_VEILLE2);
initMeasureRAZ=getSonar();
currentScenar.scenarState=RUN;
}
else
{
if(!slopeUpdate(&lampSlope)){
changeLight(int(lampSlope.currentValue));
}else
{
//Effet clignotement
// on peut fare varier la lumière ici
// int lightTmp=int(lampSlope.currentValue)===LIGHT_VEILLE?LIGHT_VEILLE_BLINK:LIGHT_VEILLE;
// slopeInit(&lampSlope, currentPosition_light, lightTmp, DELAY_LIGHT_VEILLE);
}
// si le sonar est coupé par qq1, on passe en Activation
if(getSonar() < 1500){
changeScenar(ACTIVATION);
}
}
}
/**
* Scenaria RAZ fait au démarrage du bouzin
*/
void scenarRAZ()
{
// Serial.print("initMeasureRAZ : ");
// Serial.println(initMeasureRAZ);
if (currentScenar.scenarState == INIT_SCENAR)
{
changeLight(LIGHT_RAZ);
slopeInit(&lampSlope, 0, LIGHT_VEILLE, DELAY_RAZ);
// FIXME : RAZ
// On doit faire remonter jusqu'à 0;
// on force de -10
stepper.moveTo(MOTOR_REF_0);
currentScenar.scenarState=RUN;
}
// On attend 10 secondes
// else if (diffTime(currentScenar.timeValue,DELAY_RAZ)==TRUE) // && initMeasureRAZ > 2000)
// {
// changeScenar(VEILLE);
//}
else
{
// on fait une pente pour atteindre la luminosité de VEILLE
// FIXME : il faut attendre que le sonar se stabilise
if(slopeUpdate(&lampSlope) && sonarState == STABLE){
// on va tenter du bloquant
if(stepper.currentPosition()==MOTOR_REF_0){
Serial.print(millis());
Serial.println(" move -10");
// déplacement relatif de -10
stepper.move(-10);
// fonction bloquante
stepper.runToPosition();
Serial.print(millis());
Serial.println(" After runToPosition");
stepper.setCurrentPosition(MOTOR_REF_0);
initMeasureRAZ = sonar.GetMeasureMM();
Serial.print("initMeasureRAZ : ");
Serial.println(initMeasureRAZ);
// on initialise le current position
currentPosition_motor=MOTOR_REF_0;
myTime=millis();
// on a fini la pente, on y va
changeScenar(VEILLE);
}
}else{
changeLight(int(lampSlope.currentValue));
}
}
// if (myTime - lastTimeStep > 10000 && initMeasureRAZ > 2000)
// {
// // initMeasureRAZ=sonar.GetMeasureMM();
// // Serial.print("initMeasureRAZ: ");
// // Serial.println(initMeasureRAZ);
// // oeuvreState = VEILLE;
// // scenarState = INIT;
// // stepper.moveTo(-200);
// // Serial.println("oeuvreState = VEILLE;");
// changeScenar(VEILLE);
// }
// else
// {
// // // on positionne tout en haut avec +10 pas
// // // après la position mettre en pause pour éviter que ça bascule
// // // initMeasureRAZ=sonar.GetMeasureMM();
// // if (currentPosition_light != 1)
// // {
// // lastTimeStep = myTime;
// // }
// // changeLight(1);
// // // if(stepper.getCurrentPosition()==0){
// // // on le remonte de 10
// // //}
// // // TODO : faire remonter et stopper
// // stepper.move(0);
// // stepper.setCurrentPosition(0);
// // // stepper.moveTo(0);
// // // Serial.println("oeuvreState = RAZ;");
// // // lastTimeStep = myTime;
// }
}
