├── README.md ├── .gitignore ├── examples ├── Basic │ └── Basic.ino ├── Square │ └── Square.ino └── Polygone │ └── Polygone.ino ├── library.properties ├── keywords.txt ├── library.json ├── README.adoc └── src ├── Botly.h ├── Botly.cpp ├── BotlySteppers.h └── BotlySteppers.cpp /README.md: -------------------------------------------------------------------------------- 1 | # Scott-Library 2 | Librarie Scott pour Arduino IDE 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .DS_Store 3 | .pio 4 | .vscode 5 | platformio.ini 6 | html 7 | latex -------------------------------------------------------------------------------- /examples/Basic/Basic.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Botly robot; 4 | 5 | void setup(){ 6 | robot.init(); 7 | } 8 | 9 | void loop(){ 10 | //robot.avancer(10); //Move forward by 10 mm 11 | //robot.reculer(10); //Move forward by 10 mm 12 | } -------------------------------------------------------------------------------- /examples/Square/Square.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Botly robot; 4 | 5 | void setup(){ 6 | robot.init(); 7 | } 8 | 9 | void loop(){ 10 | robot.avancer(10); //Move forward by 10 mm 11 | robot.tournerGauche(90); //Turn left by 90° 12 | } -------------------------------------------------------------------------------- /examples/Polygone/Polygone.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Botly robot; 4 | 5 | void setup(){ 6 | robot.init(); 7 | } 8 | 9 | void loop(){ 10 | polygone(4, 100); 11 | } 12 | 13 | void polygone(int nbCote, int longueurCote){ 14 | for (int i = 0; i < nbCote; i++){ 15 | robot.tournerGauche(360/nbCote); 16 | robot.avancer(longueurCote); 17 | } 18 | } -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Botly 2 | version=2.5.0 3 | author=JulesTopart, NadarBreicq, Alexandre Pecher 4 | maintainer=La Machinerie 5 | sentence=Librarie permettant de piloter le robot Botly développé par l'association La Machinerie 6 | paragraph=Library to control drawning robot Bolty. 7 | license=CC-BY-NC-SA 8 | category=Device Control 9 | url=https://github.com/Botly-Studio/Botly-Library 10 | architectures=avr 11 | includes=Botly.h 12 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | # Syntax Coloring Map Botly 2 | 3 | # Datatypes (KEYWORD1) 4 | 5 | Botly KEYWORD1 6 | 7 | # Methods and Functions (KEYWORD2) 8 | 9 | gauche KEYWORD2 10 | droite KEYWORD2 11 | avant KEYWORD2 12 | arriere KEYWORD2 13 | tournerGauche KEYWORD2 14 | tournerDroite KEYWORD2 15 | avancer KEYWORD2 16 | reculer KEYWORD2 17 | stop KEYWORD2 18 | setSpeed KEYWORD2 19 | run KEYWORD2 20 | init KEYWORD2 21 | stop KEYWORD2 22 | logSpeed KEYWORD2 23 | turnGo KEYWORD2 24 | turnGoDegree KEYWORD2 25 | polygone KEYWORD2 26 | rectangle KEYWORD2 27 | carre KEYWORD2 28 | cercle KEYWORD2 29 | arc KEYWORD2 30 | leverCrayon KEYWORD2 31 | poserCrayon KEYWORD2 32 | bougerCrayon KEYWORD2 33 | lectureDistance KEYWORD2 -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Botly", 3 | "keywords": "robot,drawing,arduino,lilypad,machinerie", 4 | "description": "Arduino Library for the Botly robot developped by La Machinerie", 5 | "repository": 6 | { 7 | "type": "git", 8 | "url": "https://github.com/LaMachinerie/Botly-Library" 9 | }, 10 | "authors": 11 | [ 12 | { 13 | "name": "La Machinerie", 14 | "email": "contact@lamachinerie.org", 15 | "url": "http://lamachinerie.org", 16 | "maintainer": true 17 | }, 18 | { 19 | "name": "Adrien BRACQ", 20 | "email": "adrien@lamachinerie.org" 21 | }, 22 | { 23 | "name": "Jules TOPART", 24 | "email": "jules@lamachinerie.org" 25 | } 26 | ], 27 | "dependencies": 28 | { 29 | 30 | }, 31 | "version": "2.5.0", 32 | "frameworks": "arduino", 33 | "platforms": "lilypad" 34 | } 35 | -------------------------------------------------------------------------------- /README.adoc: -------------------------------------------------------------------------------- 1 | = Bibliothèque Botly pour plateforme Arduino = 2 | 3 | Cette bibliothèque a pour but de faciliter le pilotage du robot Botly 4 | 5 | == License == 6 | 7 | Cette bibliothèque est distribuée sous licence Creative Commons CC-BY-NC-SA 8 | 9 | Vous êtes autorisé à : 10 | 11 | Partager — copier, distribuer et communiquer le matériel par tous moyens et sous 12 | tous formats Adapter — remixer, transformer et créer à partir du matériel 13 | L'Offrant ne peut retirer les autorisations concédées par la licence tant que 14 | vous appliquez les termes de cette licence. 15 | 16 | Attribution — Vous devez créditer l'Œuvre, intégrer un lien vers la licence et 17 | indiquer si des modifications ont été effectuées à l'Oeuvre. Vous devez indiquer 18 | ces informations par tous les moyens raisonnables, sans toutefois suggérer que 19 | l'Offrant vous soutient ou soutient la façon dont vous avez utilisé son Oeuvre. 20 | 21 | Pas d’Utilisation Commerciale — Vous n'êtes pas autorisé à faire un usage 22 | commercial de cette Oeuvre, tout ou partie du matériel la composant. 23 | 24 | Partage dans les Mêmes Conditions — Dans le cas où vous effectuez un remix, que 25 | vous transformez, ou créez à partir du matériel composant l'Oeuvre originale, 26 | vous devez diffuser l'Oeuvre modifiée dans les même conditions, c'est à dire 27 | avec la même licence avec laquelle l'Oeuvre originale a été diffusée. 28 | -------------------------------------------------------------------------------- /src/Botly.h: -------------------------------------------------------------------------------- 1 | /* 2 | .--. . . .--. . . 3 | | ) _|_ | | ) | _|_ 4 | |--: .-. | | . . ____ |--' .-. |.-. .-. | 5 | | )( ) | | | | | \ ( )| )( ) | 6 | '--' `-' `-'`-`--| ' ` `-' '`-' `-' `-' 7 | ; 8 | `-' 9 | * [foo description] 10 | * Librairie principale des robots Botly et Scott 11 | * @date 2018-11-22 12 | * @author Jules T. / Adrien B. / Alexandre P. 13 | * @entreprise La Machinerie 14 | * @version V2.1.0 15 | */ 16 | 17 | #ifndef Botly_h 18 | #define Botly_h 19 | #define LIBRARY_VERSION 2.5.0 20 | 21 | /********************************* 22 | Constantes de calibrations 23 | *********************************/ 24 | 25 | #define BOTLY_MM_TO_STEP 345 26 | #define BOTLY_RAD_TO_STEP 1861 27 | #define BOTLY_DELTA_ARC 47 28 | #define NEW_CALIBRATION 66 29 | 30 | #define DIR_RIGHT 1 31 | #define DIR_LEFT 2 32 | 33 | /********************* 34 | Dependances 35 | *********************/ 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | /* Desactivated Incompatibilty with latest version of IRremote 43 | #include 44 | */ 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | #include "BotlySteppers.h" 51 | 52 | //void pin2_isr(); 53 | 54 | class Botly{ 55 | public: 56 | 57 | Servo crayon; 58 | 59 | //TODO 60 | //IRsend irsend; 61 | //decode_results results; 62 | 63 | //TODO 64 | /* 65 | int pin = 9; 66 | IRrecv irrecv = new IRrecv(pin); 67 | */ 68 | 69 | Botly(); 70 | 71 | Botly(int version); 72 | 73 | void init(); 74 | 75 | void setCalibration(int distance, int rotation); 76 | 77 | void getCalibration(); 78 | 79 | void factoryCalibration(); 80 | 81 | void run(); 82 | 83 | void stop(long temps); 84 | 85 | void stop(); 86 | 87 | void gauche(long pas); 88 | 89 | void tournerGauche(long angleDegree); 90 | 91 | void droite(long pas); 92 | 93 | void tournerDroite(long angleDegree); 94 | 95 | void avant(long pas); 96 | 97 | void avancer(long distanceMillimeter); 98 | 99 | void arriere(long pas); 100 | 101 | void reculer(long distanceMillimeter); 102 | 103 | void setSpeed(float vitesse); 104 | 105 | void setSpeed(float vitesseD, float vitesseG); 106 | 107 | void logSpeed(); 108 | 109 | void turnGo(float angle, long ligne); 110 | 111 | void turnGoDegree(float angle, long ligne); 112 | 113 | void polygone(unsigned int nbrCote, unsigned int longueur, unsigned int dir = DIR_RIGHT); 114 | 115 | void rectangle(unsigned int largeur, unsigned int longueur); 116 | 117 | void carre(unsigned int longueur); 118 | 119 | void cercle(unsigned int diametre, unsigned int dir = DIR_RIGHT); 120 | 121 | void arc(float rayon,float angle); 122 | 123 | void leverCrayon(); 124 | 125 | void poserCrayon(); 126 | 127 | void bougerCrayon(int angle); 128 | 129 | void finProgramme(); 130 | 131 | void musicBegin(); 132 | 133 | void musicNewProgramm(); 134 | 135 | void musicEnd(); 136 | 137 | //TODO 138 | /* IR dependent 139 | void isIRDataReceived(); 140 | 141 | void initIRcom(); 142 | 143 | void sonyCode(byte data); 144 | 145 | bool proximite(int i = 10, int seuil = 5); 146 | */ 147 | 148 | int mesureBatterie(); 149 | 150 | void sleepNow(); 151 | 152 | void sleepWakeup(); 153 | 154 | private: 155 | 156 | int _pinBotlyServo= 11 ; // Pin servo pour BotlyV1 157 | 158 | // Définition des pins à partir de la version BotlyV1 159 | int _pinTsop = 9; 160 | int _pinBotlyIrEmetteur = 13 ; 161 | int _pinMesureBatterie = A5; 162 | int _pinBuzzer = 7; 163 | BotlySteppers *Steppers; 164 | 165 | // Variable capteur de distance 166 | int _distDroite; 167 | int _distGauche; 168 | 169 | // Variable Crayon 170 | int _botlyBas = 90; 171 | int _botlyHaut = 20; 172 | 173 | // Variables de calibration des deplacements 174 | int _mmToStep = BOTLY_MM_TO_STEP ; 175 | int _radToStep = BOTLY_RAD_TO_STEP ; 176 | int _deltaArc = 0; 177 | 178 | int servoAction = 0; 179 | 180 | // gestion de _version 181 | byte _buildSec = 0; 182 | 183 | // Adresses de stockage EEPROM 184 | byte _timeAddress = 0; 185 | byte _distanceAddress = 5; 186 | byte _rotationAddress = 10; 187 | byte _checkNewAddress = 15; 188 | 189 | }; 190 | #endif 191 | -------------------------------------------------------------------------------- /src/Botly.cpp: -------------------------------------------------------------------------------- 1 | #include "Botly.h" 2 | 3 | 4 | Botly::Botly(){ 5 | Steppers = new BotlySteppers(); 6 | } 7 | 8 | void Botly::init() 9 | { 10 | 11 | Serial.begin(9600); 12 | analogReference(INTERNAL); //reference analogique 2.56V 13 | 14 | crayon.attach(_pinBotlyServo); 15 | crayon.write(_botlyHaut); 16 | 17 | pinMode(_pinBotlyIrEmetteur, OUTPUT); 18 | digitalWrite(_pinBotlyIrEmetteur, LOW); 19 | 20 | getCalibration(); 21 | _deltaArc = BOTLY_DELTA_ARC; 22 | 23 | // Detecte si une nouvelle version du code uploadé 24 | // Atention : ne fonctionne pas encore. 25 | // Utilisé uniquement à la recompilation de la library 26 | if(__TIME__[0] == '?') _buildSec = 99; 27 | else _buildSec = ((__TIME__[6] - '0') * 10 + __TIME__[7] - '0') ; 28 | 29 | if (EEPROM.read(_timeAddress)!=_buildSec) 30 | { 31 | EEPROM.update(_timeAddress, _buildSec); 32 | musicNewProgramm(); 33 | } 34 | else musicBegin(); 35 | 36 | Steppers->setMaxSpeed(900.0); 37 | Steppers->setSpeed(300.0); 38 | Steppers->enable(); 39 | 40 | } 41 | 42 | void Botly::run(){ 43 | Steppers->run(); 44 | } 45 | 46 | void Botly::setCalibration(int distance, int rotation){ 47 | 48 | // Mise à jour des variables de calibration 49 | _mmToStep = distance; 50 | _radToStep = rotation; 51 | 52 | // Sauvegarde des paramètres dans l'EEPROM 53 | EEPROM.update(_distanceAddress, distance >> 8); 54 | EEPROM.update(_distanceAddress+1, distance & 255); 55 | 56 | EEPROM.update(_rotationAddress, rotation >> 8); 57 | EEPROM.update(_rotationAddress+1, rotation & 255); 58 | 59 | // Sauvegarde du changement de la calibration 60 | EEPROM.update(_checkNewAddress, NEW_CALIBRATION); 61 | } 62 | 63 | void Botly::getCalibration(){ 64 | 65 | // Si une nouvelle valeur a été chargée dans le robot prendre ces valeurs 66 | if (EEPROM.read(_checkNewAddress) == NEW_CALIBRATION) 67 | { 68 | _mmToStep = EEPROM.read(_distanceAddress) << 8 | EEPROM.read(_distanceAddress+1); 69 | _radToStep = EEPROM.read(_rotationAddress) << 8 | EEPROM.read(_rotationAddress+1); 70 | } 71 | else 72 | { 73 | _mmToStep = BOTLY_MM_TO_STEP ; 74 | _radToStep = BOTLY_RAD_TO_STEP ; 75 | } 76 | } 77 | 78 | void Botly::factoryCalibration(){ 79 | // revenir à la calibration usine 80 | EEPROM.update(_checkNewAddress, 0); 81 | _mmToStep = BOTLY_MM_TO_STEP ; 82 | _radToStep = BOTLY_RAD_TO_STEP ; 83 | } 84 | 85 | void Botly::setSpeed(float vitesse){ 86 | Steppers->setSpeed(vitesse); 87 | } 88 | 89 | void Botly::setSpeed( float vitesseD, float vitesseG){ 90 | Steppers->setSpeed(vitesseD, vitesseG); 91 | } 92 | 93 | void Botly::logSpeed(){ 94 | Serial.print("Vitesse : "); Serial.print(Steppers->getSpeed(0)); Serial.print(" | "); Serial.println(Steppers->getSpeed(1)); 95 | Serial.print("Vitesse max : "); Serial.print(Steppers->getMaxSpeed(0)); Serial.print(" | "); Serial.println(Steppers->getMaxSpeed(1)); 96 | } 97 | 98 | 99 | void Botly::turnGoDegree(float angle, long ligne){ 100 | angle = angle * DEG_TO_RAD ; // Passage en radians 101 | turnGo(angle, ligne); 102 | } 103 | 104 | void Botly::turnGo(float angle, long ligne){ 105 | 106 | if(angle > 0 && angle < PI){ 107 | gauche( int( (angle * _radToStep)) ); 108 | } 109 | else if( angle >= PI ){ 110 | droite(int( ( (angle-PI) * _radToStep)) ); 111 | } 112 | else if( angle < 0 ){ 113 | droite(int( -( angle * _radToStep)) ); 114 | } 115 | else{ 116 | stop(100); 117 | } 118 | 119 | if( ligne > 0 ){ 120 | avant( (ligne * _mmToStep)/10 ); 121 | } 122 | else if( ligne < 0 ){ 123 | arriere( -( ligne * _mmToStep)/10 ); 124 | } 125 | else{ 126 | stop(100); 127 | } 128 | } 129 | 130 | void Botly::musicBegin(){ 131 | 132 | delay(500); 133 | tone(_pinBuzzer, 1364, 100); 134 | delay(110); 135 | tone(_pinBuzzer, 1535, 250); 136 | delay(300); 137 | tone(_pinBuzzer, 2060, 500); 138 | delay(500); 139 | 140 | } 141 | 142 | void Botly::musicNewProgramm(){ 143 | 144 | tone(_pinBuzzer, 1364, 500); 145 | delay(400); 146 | tone(_pinBuzzer, 1364, 80); 147 | delay(100); 148 | tone(_pinBuzzer, 1364, 80); 149 | delay(100); 150 | tone(_pinBuzzer, 2060, 800); 151 | delay(800); 152 | } 153 | 154 | void Botly::musicEnd(){ 155 | 156 | delay(500); 157 | tone(_pinBuzzer, 1976-33, 500); 158 | delay(500); 159 | tone(_pinBuzzer, 1480-33, 100); 160 | delay(110); 161 | tone(_pinBuzzer, 1568-33, 800); 162 | delay(800); 163 | } 164 | 165 | void Botly::avant(long pas){ 166 | Steppers->moveTo(-pas, pas); 167 | Steppers->runSpeedToPosition(); //Blockling... 168 | Steppers->setPositions(); 169 | } 170 | 171 | void Botly::arriere(long pas){ 172 | Steppers->moveTo(pas, -pas); 173 | Steppers->runSpeedToPosition();//Blockling... 174 | Steppers->setPositions(); 175 | } 176 | 177 | void Botly::gauche(long pas){ 178 | Steppers->moveTo(-pas, -pas); 179 | Steppers->runSpeedToPosition();//Blockling... 180 | Steppers->setPositions(); 181 | 182 | } 183 | 184 | void Botly::droite(long pas){ 185 | Steppers->moveTo(pas, pas); 186 | Steppers->runSpeedToPosition();//Blockling... 187 | Steppers->setPositions(); 188 | } 189 | 190 | 191 | //Battery Power save !!!! 192 | void Botly::stop(long temps){ 193 | if(temps > 40){ 194 | delay(20); 195 | Steppers->disable(); 196 | delay(temps-40); 197 | Steppers->enable(); 198 | delay(20); 199 | }else{ 200 | delay(temps); 201 | } 202 | } 203 | 204 | //Battery Power save !!!! 205 | void Botly::stop(){ 206 | Steppers->disable(); 207 | while(true); 208 | } 209 | 210 | void Botly::tournerGauche(long angleDegree){ 211 | gauche(long((angleDegree * DEG_TO_RAD * _radToStep))); 212 | } 213 | 214 | void Botly::tournerDroite(long angleDegree){ 215 | droite(long((angleDegree * DEG_TO_RAD *_radToStep))); 216 | } 217 | 218 | void Botly::avancer(long distanceMillimeter){ 219 | turnGoDegree(0, distanceMillimeter); 220 | } 221 | 222 | void Botly::reculer(long distanceMillimeter){ 223 | turnGoDegree(0, -distanceMillimeter); 224 | } 225 | 226 | void Botly::polygone(unsigned int nbrCote, unsigned int longueur, unsigned int direction){ 227 | if (nbrCote >=3) 228 | { 229 | float polyAngle = 360 / nbrCote; 230 | if(direction != DIR_RIGHT) polyAngle = -polyAngle; 231 | turnGoDegree(0,longueur); 232 | for (unsigned int i=1 ; i 0){ 261 | pasD = ((rayon - _deltaArc) * angle*DEG_TO_RAD) * (_mmToStep/10); 262 | pasG = ((rayon + _deltaArc) * angle*DEG_TO_RAD) * (_mmToStep/10); 263 | }else{ 264 | pasG = ((rayon - _deltaArc) * angle*DEG_TO_RAD) * (_mmToStep/10); 265 | pasD = ((rayon + _deltaArc) * angle*DEG_TO_RAD) * (_mmToStep/10); 266 | } 267 | Steppers->moveTo(pasD, pasG); 268 | } 269 | 270 | void Botly::leverCrayon(){ 271 | crayon.write(_botlyHaut); 272 | } 273 | 274 | void Botly::poserCrayon(){ 275 | crayon.write(_botlyBas); 276 | } 277 | 278 | void Botly::bougerCrayon(int angle) 279 | { 280 | crayon.write(angle); 281 | } 282 | 283 | void Botly::finProgramme() 284 | { 285 | stop(); 286 | musicEnd(); 287 | while(true); 288 | } 289 | 290 | //TODO 291 | /* 292 | void Botly::isIRDataReceived(){ 293 | if (irrecv.decode(&results)) { 294 | Serial.println(results.value, HEX); 295 | irrecv.resume(); // Receive the next value 296 | } 297 | } 298 | 299 | void Botly::initIRcom(){ 300 | irrecv.enableIRIn(); // Start the receiver 301 | } 302 | 303 | void Botly::sonyCode(byte data){ 304 | irsend.sendSony(data, 8); 305 | } 306 | 307 | 308 | bool Botly::proximite(int ite, int trigger) 309 | { 310 | int validDetection = 0; 311 | trigger = (trigger > ite) ? ite : trigger ; 312 | 313 | for (int k = 0; k<= ite; k++) 314 | { 315 | delay(20); // Attendre avant une lecture 316 | 317 | // Generation des pulsation à 38kHz 318 | // Le temps à l'etat haut est diminué afin de limiter 319 | // la portée de la detection du capteur 320 | 321 | for(int i = 0; i <= 31; i++) 322 | { 323 | digitalWrite(_pinBotlyIrEmetteur, HIGH); 324 | delayMicroseconds(8); 325 | digitalWrite(_pinBotlyIrEmetteur, LOW); 326 | delayMicroseconds(13); 327 | if(digitalRead(_pinTsop)==LOW) 328 | { 329 | validDetection++; 330 | break; 331 | } 332 | } 333 | } 334 | return (validDetection>=trigger); 335 | } 336 | 337 | */ 338 | 339 | /*Cette fonction mesure la valeur analogique 340 | La référence interne est de 2.56V peu importe la tension 341 | d'alimentation du controleur 342 | Mesure sur 1024 valeurs (10bits) 343 | Méthode de recalcul: (2.56/1024)*mesureAnalogique 344 | Ne pas oublier le rapport de transformation du pont diviseur 345 | en hardware 346 | */ 347 | int Botly::mesureBatterie() 348 | { 349 | int mesureAnalogique=analogRead(_pinMesureBatterie); 350 | return mesureAnalogique; 351 | } 352 | 353 | void Botly::sleepNow() 354 | { 355 | /* In the Atmega32u4 datasheet on page 62 356 | * there is a list of sleep modes which explains which clocks and 357 | * wake up sources are available for each sleep mode. 358 | * 359 | * In the avr/sleep.h file, the call names of these sleep modus are to be found: 360 | * 361 | * The 5 different modes are: 362 | * SLEEP_MODE_IDLE -the least power savings, wake up on usart 363 | * SLEEP_MODE_ADC 364 | * SLEEP_MODE_PWR_SAVE 365 | * SLEEP_MODE_STANDBY 366 | * SLEEP_MODE_PWR_DOWN -the most power savings 367 | */ 368 | 369 | set_sleep_mode(SLEEP_MODE_PWR_DOWN); // sleep mode is set here 370 | sleep_enable(); // enables the sleep in mcucr register,safety bit 371 | power_all_disable(); //low power consumption 372 | sleep_mode(); // here the device is actually put to sleep!! 373 | // THE PROGRAM CONTINUES FROM HERE AFTER WAKING UP 374 | sleep_disable(); //disable sleep, safety bit 375 | power_all_enable(); //restore power on peripherals 376 | } 377 | 378 | void Botly::sleepWakeup() 379 | { 380 | /* In the Atmega32u4 datasheet on page 62 381 | * there is a list of sleep modes which explains which clocks and 382 | * wake up sources are available for each sleep mode. 383 | * 384 | * In the avr/sleep.h file, the call names of these sleep modus are to be found: 385 | * 386 | * The 5 different modes are: 387 | * SLEEP_MODE_IDLE -the least power savings, wake up on usart 388 | * SLEEP_MODE_ADC 389 | * SLEEP_MODE_PWR_SAVE 390 | * SLEEP_MODE_STANDBY 391 | * SLEEP_MODE_PWR_DOWN -the most power savings 392 | */ 393 | 394 | //TODO IR dependent 395 | //attachInterrupt(0, pin2_isr,LOW );//wake up on interrupt pin 2 or 3 396 | set_sleep_mode(SLEEP_MODE_PWR_DOWN); // sleep mode 397 | cli(); //clear interrupts 398 | sleep_enable(); // enables sleep bit in mcucr register, safety pin 399 | sei();//to allow wake up by interrupt 400 | //power_adc_disable(); 401 | //power_usart0_disable(); 402 | //power_spi_disable(); 403 | //power_twi_disable(); 404 | //power_timer0_disable(); 405 | //power_timer1_disable(); 406 | //power_timer2_disable(); 407 | //power_timer3_disable(); 408 | //power_usart1_disable(); 409 | //power_usb_disable(); 410 | 411 | power_all_disable(); //low power consumption 412 | sleep_mode(); // here the device is actually put to sleep!! 413 | // THE PROGRAM CONTINUES FROM HERE AFTER WAKING UP 414 | sleep_disable(); //disable sleep,safety bit 415 | power_all_enable(); //restore power on peripherals 416 | } 417 | 418 | 419 | // !!!! Pas utilisé pour le moment 420 | //external interrupt routine to wake up controller 421 | // void pin2_isr() 422 | // { 423 | // if (_version==SCOTT_V4) return; // annule la fonction si mauvaise version 424 | // sleep_disable(); 425 | // detachInterrupt(0); 426 | // } 427 | -------------------------------------------------------------------------------- /src/BotlySteppers.h: -------------------------------------------------------------------------------- 1 | #ifndef BotlySteppers_h 2 | #define BotlySteppers_h 3 | 4 | #include 5 | #if ARDUINO >= 100 6 | #include 7 | #else 8 | #include 9 | #include 10 | #endif 11 | 12 | #undef round 13 | 14 | #define MULTISTEPPER_MAX_STEPPERS 2 15 | 16 | 17 | // // PIN MOTEUR BOTLY ( A partir de la version 1) 18 | // #define BotlyGaucheB2 3 19 | // #define BotlyGaucheB1 2 20 | // #define BotlyGaucheA2 0 21 | // #define BotlyGaucheA1 1 22 | // 23 | // #define BotlyDroitB2 6 24 | // #define BotlyDroitB1 8 25 | // #define BotlyDroitA2 4 26 | // #define BotlyDroitA1 12 27 | 28 | 29 | /***************************************************** 30 | * Déifinition de la classe * 31 | * SSteppers * 32 | * * 33 | *****************************************************/ 34 | 35 | 36 | class SStepper 37 | { 38 | public: 39 | 40 | SStepper(uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5); 41 | 42 | void moveTo(long absolute); 43 | 44 | /// Set the target position relative to the current position 45 | /// \param[in] relative The desired position relative to the current position. Negative is 46 | /// anticlockwise from the current position. 47 | void move(long relative); 48 | 49 | /// Poll the motor and step it if a step is due, implementing 50 | /// accelerations and decelerations to acheive the target position. You must call this as 51 | /// frequently as possible, but at least once per minimum step time interval, 52 | /// preferably in your main loop. Note that each call to run() will make at most one step, and then only when a step is due, 53 | /// based on the current speed and the time since the last step. 54 | /// \return true if the motor is still running to the target position. 55 | boolean run(); 56 | 57 | /// Poll the motor and step it if a step is due, implementing a constant 58 | /// speed as set by the most recent call to setSpeed(). You must call this as 59 | /// frequently as possible, but at least once per step interval, 60 | /// \return true if the motor was stepped. 61 | boolean runSpeed(); 62 | 63 | /// Sets the maximum permitted speed. The run() function will accelerate 64 | /// up to the speed set by this function. 65 | /// Caution: the maximum speed achievable depends on your processor and clock speed. 66 | /// \param[in] speed The desired maximum speed in steps per second. Must 67 | /// be > 0. Caution: Speeds that exceed the maximum speed supported by the processor may 68 | /// Result in non-linear accelerations and decelerations. 69 | void setMaxSpeed(float speed); 70 | 71 | /// returns the maximum speed configured for this stepper 72 | /// that was previously set by setMaxSpeed(); 73 | /// \return The currently configured maximum speed 74 | float maxSpeed(); 75 | 76 | /// Sets the desired constant speed for use with runSpeed(). 77 | /// \param[in] speed The desired constant speed in steps per 78 | /// second. Positive is clockwise. Speeds of more than 1000 steps per 79 | /// second are unreliable. Very slow speeds may be set (eg 0.00027777 for 80 | /// once per hour, approximately. Speed accuracy depends on the Arduino 81 | /// crystal. Jitter depends on how frequently you call the runSpeed() function. 82 | void setSpeed(float speed); 83 | 84 | /// The most recently set speed 85 | /// \return the most recent speed in steps per second 86 | float speed(); 87 | 88 | /// The distance from the current position to the target position. 89 | /// \return the distance from the current position to the target position 90 | /// in steps. Positive is clockwise from the current position. 91 | long distanceToGo(); 92 | 93 | /// The most recently set target position. 94 | /// \return the target position 95 | /// in steps. Positive is clockwise from the 0 position. 96 | long targetPosition(); 97 | 98 | /// The currently motor position. 99 | /// \return the current motor position 100 | /// in steps. Positive is clockwise from the 0 position. 101 | long currentPosition(); 102 | 103 | /// Resets the current position of the motor, so that wherever the motor 104 | /// happens to be right now is considered to be the new 0 position. Useful 105 | /// for setting a zero position on a stepper after an initial hardware 106 | /// positioning move. 107 | /// Has the side effect of setting the current motor speed to 0. 108 | /// \param[in] position The position in steps of wherever the motor 109 | /// happens to be right now. 110 | void setCurrentPosition(long position); 111 | 112 | /// Moves the motor (with acceleration/deceleration) 113 | /// to the target position and blocks until it is at 114 | /// position. Dont use this in event loops, since it blocks. 115 | void runToPosition(); 116 | 117 | /// Runs at the currently selected speed until the target position is reached 118 | /// Does not implement accelerations. 119 | /// \return true if it stepped 120 | boolean runSpeedToPosition(); 121 | 122 | /// Moves the motor (with acceleration/deceleration) 123 | /// to the new target position and blocks until it is at 124 | /// position. Dont use this in event loops, since it blocks. 125 | /// \param[in] position The new target position. 126 | void runToNewPosition(long position); 127 | 128 | /// Sets a new target position that causes the stepper 129 | /// to stop as quickly as possible, using the current speed and acceleration parameters. 130 | void stop(); 131 | 132 | /// Disable motor pin outputs by setting them all LOW 133 | /// Depending on the design of your electronics this may turn off 134 | /// the power to the motor coils, saving power. 135 | /// This is useful to support Arduino low power modes: disable the outputs 136 | /// during sleep and then reenable with enableOutputs() before stepping 137 | /// again. 138 | /// If the enable Pin is defined, sets it to OUTPUT mode and clears the pin to disabled. 139 | virtual void disableOutputs(); 140 | 141 | /// Enable motor pin outputs by setting the motor pins to OUTPUT 142 | /// mode. Called automatically by the constructor. 143 | /// If the enable Pin is defined, sets it to OUTPUT mode and sets the pin to enabled. 144 | virtual void enableOutputs(); 145 | 146 | /// Sets the minimum pulse width allowed by the stepper driver. The minimum practical pulse width is 147 | /// approximately 20 microseconds. Times less than 20 microseconds 148 | /// will usually result in 20 microseconds or so. 149 | /// \param[in] minWidth The minimum pulse width in microseconds. 150 | void setMinPulseWidth(unsigned int minWidth); 151 | 152 | /// Sets the enable pin number for stepper drivers. 153 | /// 0xFF indicates unused (default). 154 | /// Otherwise, if a pin is set, the pin will be turned on when 155 | /// enableOutputs() is called and switched off when disableOutputs() 156 | /// is called. 157 | /// \param[in] enablePin Arduino digital pin number for motor enable 158 | /// \sa setPinsInverted 159 | void setEnablePin(uint8_t enablePin = 0xff); 160 | 161 | /// Sets the inversion for stepper driver pins 162 | /// \param[in] directionInvert True for inverted direction pin, false for non-inverted 163 | /// \param[in] stepInvert True for inverted step pin, false for non-inverted 164 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 165 | void setPinsInverted(bool directionInvert = false, bool stepInvert = false, bool enableInvert = false); 166 | 167 | /// Sets the inversion for 2, 3 and 4 wire stepper pins 168 | /// \param[in] pin1Invert True for inverted pin1, false for non-inverted 169 | /// \param[in] pin2Invert True for inverted pin2, false for non-inverted 170 | /// \param[in] pin3Invert True for inverted pin3, false for non-inverted 171 | /// \param[in] pin4Invert True for inverted pin4, false for non-inverted 172 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 173 | void setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert); 174 | 175 | /// Checks to see if the motor is currently running to a target 176 | /// \return true if the speed is not zero or not at the target position 177 | bool isRunning(); 178 | 179 | protected: 180 | 181 | /// \brief Direction indicator 182 | /// Symbolic names for the direction the motor is turning 183 | typedef enum 184 | { 185 | DIRECTION_CCW = 0, ///< Counter-Clockwise 186 | DIRECTION_CW = 1 ///< Clockwise 187 | } Direction; 188 | 189 | 190 | void computeNewSpeed(); 191 | 192 | virtual void setOutputPins(uint8_t mask); 193 | 194 | virtual void step(long step); 195 | 196 | 197 | boolean _direction; // 1 == CW 198 | 199 | private: 200 | 201 | /// Arduino pin number assignments for the 2 or 4 pins required to interface to the 202 | /// stepper motor or driver 203 | uint8_t _pin[4]; 204 | 205 | /// Whether the _pins is inverted or not 206 | uint8_t _pinInverted[4]; 207 | 208 | /// The current absolution position in steps. 209 | long _currentPos; // Steps 210 | 211 | /// The target position in steps. The AccelStepper library will move the 212 | /// motor from the _currentPos to the _targetPos, taking into account the 213 | /// max speed, acceleration and deceleration 214 | long _targetPos; // Steps 215 | 216 | /// The current motos speed in steps per second 217 | /// Positive is clockwise 218 | float _speed; // Steps per second 219 | 220 | /// The maximum permitted speed in steps per second. Must be > 0. 221 | float _maxSpeed; 222 | 223 | /// The acceleration to use to accelerate or decelerate the motor in steps 224 | /// per second per second. Must be > 0 225 | float _acceleration; 226 | float _sqrt_twoa; // Precomputed sqrt(2*_acceleration) 227 | 228 | /// The current interval between steps in microseconds. 229 | /// 0 means the motor is currently stopped with _speed == 0 230 | unsigned long _stepInterval; 231 | 232 | /// The last step time in microseconds 233 | unsigned long _lastStepTime; 234 | 235 | /// The minimum allowed pulse width in microseconds 236 | unsigned int _minPulseWidth; 237 | 238 | /// Is the direction pin inverted? 239 | ///bool _dirInverted; /// Moved to _pinInverted[1] 240 | 241 | /// Is the step pin inverted? 242 | ///bool _stepInverted; /// Moved to _pinInverted[0] 243 | 244 | /// Is the enable pin inverted? 245 | bool _enableInverted; 246 | 247 | /// Enable pin for stepper driver, or 0xFF if unused. 248 | uint8_t _enablePin; 249 | 250 | /// The step counter for speed calculations 251 | long _n; 252 | 253 | /// Initial step size in microseconds 254 | float _c0; 255 | 256 | /// Last step size in microseconds 257 | float _cn; 258 | 259 | /// Min step size in microseconds based on maxSpeed 260 | float _cmin; // at max speed 261 | 262 | }; 263 | 264 | /***************************************************** 265 | * Déifinition de la classe * 266 | * BotlySteppers * 267 | * * 268 | *****************************************************/ 269 | 270 | 271 | 272 | class BotlySteppers{ 273 | public: 274 | 275 | BotlySteppers(); 276 | 277 | bool run(); 278 | 279 | void move(long relativeD, long relativeG); 280 | void moveTo(long absoluteD, long absoluteG); 281 | 282 | void runSpeedToPosition(); 283 | void setPositions(); 284 | 285 | void setSpeed(float vitesse); 286 | void setSpeed(float vitesseDroite, float vitesseGauche); 287 | 288 | void setMaxSpeed(float vitesse); 289 | void setMaxSpeed(float vitesseDroite, float vitesseGauche); 290 | 291 | float getMaxSpeed(int i); 292 | float getSpeed(int i); 293 | 294 | void disable(); 295 | void enable(); 296 | 297 | private: 298 | 299 | SStepper _stepperD; 300 | SStepper _stepperG; 301 | 302 | // PIN MOTEUR BOTLY ( A partir de la version 1) 303 | const uint8_t BotlyGaucheB2 = 3, BotlyGaucheB1 = 2, BotlyGaucheA2 = 0, BotlyGaucheA1 = 1 ; 304 | const uint8_t BotlyDroitB2 = 6, BotlyDroitB1 = 8, BotlyDroitA2 = 4, BotlyDroitA1 = 12 ; 305 | 306 | }; 307 | 308 | 309 | #endif 310 | -------------------------------------------------------------------------------- /src/BotlySteppers.cpp: -------------------------------------------------------------------------------- 1 | #include "BotlySteppers.h" 2 | 3 | /***************************************************** 4 | * Méthodes et Constructeur * 5 | * BotlySteppers * 6 | * * 7 | *****************************************************/ 8 | 9 | BotlySteppers::BotlySteppers() 10 | { 11 | SStepper droite(BotlyDroitB2, BotlyDroitA2, BotlyDroitB1, BotlyDroitA1); 12 | SStepper gauche(BotlyGaucheB2, BotlyGaucheA2, BotlyGaucheB1, BotlyGaucheA1); 13 | _stepperD = droite; 14 | _stepperG = gauche; 15 | } 16 | 17 | bool BotlySteppers::run() 18 | { 19 | uint8_t i; 20 | bool ret = false; 21 | if ( _stepperD.distanceToGo() != 0) 22 | { 23 | _stepperD.runSpeed(); 24 | ret = true; 25 | } 26 | if ( _stepperG.distanceToGo() != 0) 27 | { 28 | _stepperG.runSpeed(); 29 | ret = true; 30 | } 31 | return ret; 32 | } 33 | 34 | 35 | void BotlySteppers::runSpeedToPosition() 36 | { 37 | while (run()) 38 | ; 39 | } 40 | 41 | void BotlySteppers::setPositions(){ 42 | _stepperD.setCurrentPosition(0); 43 | _stepperG.setCurrentPosition(0); 44 | } 45 | 46 | void BotlySteppers::moveTo(long absoluteD, long absoluteG){ 47 | // First find the stepper that will take the longest time to move 48 | float longestTime = 0.0; 49 | 50 | uint8_t i; 51 | 52 | long thisDistance = absoluteD - _stepperD.currentPosition(); 53 | float thisTime = abs(thisDistance) / _stepperD.maxSpeed(); 54 | 55 | if (thisTime > longestTime) 56 | longestTime = thisTime; 57 | 58 | 59 | thisDistance = absoluteG - _stepperG.currentPosition(); 60 | thisTime = abs(thisDistance) / _stepperG.maxSpeed(); 61 | 62 | if (thisTime > longestTime) 63 | longestTime = thisTime; 64 | 65 | 66 | if (longestTime > 0.0) 67 | { 68 | // Now work out a new max speed for each stepper so they will all 69 | // arrived at the same time of longestTime 70 | 71 | thisDistance = absoluteD - _stepperD.currentPosition(); 72 | float thisSpeed = thisDistance / longestTime; 73 | _stepperD.moveTo(absoluteD); // New target position (resets speed) 74 | _stepperD.setSpeed(thisSpeed); // New speed 75 | 76 | thisDistance = absoluteG - _stepperG.currentPosition(); 77 | thisSpeed = thisDistance / longestTime; 78 | _stepperG.moveTo(absoluteG); // New target position (resets speed) 79 | _stepperG.setSpeed(thisSpeed); // New speed 80 | 81 | } 82 | } 83 | 84 | void BotlySteppers::move(long relativeD, long relativeG){ 85 | //float thisSpeed = _stepperD.maxSpeed(); 86 | _stepperD.move(relativeD); // New target position (resets speed) 87 | //_stepperD.setSpeed(thisSpeed); // New speed 88 | 89 | //thisSpeed = _stepperG.maxSpeed(); 90 | _stepperG.move(relativeG); // New target position (resets speed) 91 | //_stepperG.setSpeed(thisSpeed); // New speed 92 | } 93 | 94 | float BotlySteppers::getMaxSpeed(int i){ 95 | if(i == 1) return _stepperD.maxSpeed(); 96 | if(i == 2) return _stepperG.maxSpeed(); 97 | return 0; 98 | } 99 | 100 | float BotlySteppers::getSpeed(int i){ 101 | if(i == 1) return _stepperD.speed(); 102 | if(i == 2) return _stepperG.speed(); 103 | return 0; 104 | } 105 | 106 | 107 | void BotlySteppers::setMaxSpeed(float vitesse){ 108 | setMaxSpeed(vitesse, vitesse); 109 | } 110 | 111 | void BotlySteppers::setSpeed(float vitesse){ 112 | setSpeed(vitesse, vitesse); 113 | } 114 | 115 | void BotlySteppers::setMaxSpeed(float vitesseDroite, float vitesseGauche){ 116 | _stepperD.setMaxSpeed(vitesseDroite); 117 | _stepperG.setMaxSpeed(vitesseGauche); 118 | } 119 | 120 | void BotlySteppers::setSpeed(float vitesseDroite, float vitesseGauche){ 121 | _stepperD.setSpeed(vitesseDroite); 122 | _stepperG.setSpeed(vitesseGauche); 123 | } 124 | 125 | 126 | void BotlySteppers::disable(){ 127 | _stepperD.disableOutputs(); 128 | _stepperG.disableOutputs(); 129 | } 130 | 131 | void BotlySteppers::enable(){ 132 | _stepperD.enableOutputs(); 133 | _stepperG.enableOutputs(); 134 | } 135 | 136 | 137 | 138 | 139 | 140 | //Fin de la class BotlySteppers 141 | 142 | 143 | 144 | 145 | 146 | /***************************************************** 147 | * Méthodes et Constructeur * 148 | * SSteppers * 149 | * * 150 | *****************************************************/ 151 | 152 | void SStepper::moveTo(long absolute) 153 | { 154 | if (_targetPos != absolute) 155 | { 156 | _targetPos = absolute; 157 | computeNewSpeed(); 158 | // compute new n? 159 | } 160 | } 161 | 162 | void SStepper::move(long relative) 163 | { 164 | moveTo(_currentPos + relative); 165 | } 166 | 167 | // Implements steps according to the current step interval 168 | // You must call this at least once per step 169 | // returns true if a step occurred 170 | bool SStepper::runSpeed() 171 | { 172 | // Dont do anything unless we actually have a step interval 173 | if (!_stepInterval) 174 | return false; 175 | 176 | unsigned long time = micros(); 177 | if (time - _lastStepTime >= _stepInterval){ 178 | if (_direction == DIRECTION_CW){ 179 | // Clockwise 180 | _currentPos += 1; 181 | } 182 | else{ 183 | // Anticlockwise 184 | _currentPos -= 1; 185 | } 186 | step(_currentPos); 187 | 188 | _lastStepTime = time; // Caution: does not account for costs in step() 189 | 190 | return true; 191 | } 192 | else{ 193 | return false; 194 | } 195 | } 196 | 197 | long SStepper::distanceToGo() 198 | { 199 | return _targetPos - _currentPos; 200 | } 201 | 202 | long SStepper::targetPosition() 203 | { 204 | return _targetPos; 205 | } 206 | 207 | long SStepper::currentPosition() 208 | { 209 | return _currentPos; 210 | } 211 | 212 | // Useful during initialisations or after initial positioning 213 | // Sets speed to 0 214 | void SStepper::setCurrentPosition(long position) 215 | { 216 | _targetPos = _currentPos = position; 217 | _n = 0; 218 | _stepInterval = 0; 219 | _speed = 0.0; 220 | } 221 | 222 | void SStepper::computeNewSpeed() 223 | { 224 | long distanceTo = distanceToGo(); // +ve is clockwise from curent location 225 | 226 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 227 | 228 | if (distanceTo == 0 && stepsToStop <= 1) 229 | { 230 | // We are at the target and its time to stop 231 | _stepInterval = 0; 232 | _speed = 0.0; 233 | _n = 0; 234 | return; 235 | } 236 | 237 | if (distanceTo > 0) 238 | { 239 | // We are anticlockwise from the target 240 | // Need to go clockwise from here, maybe decelerate now 241 | if (_n > 0) 242 | { 243 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 244 | if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW) 245 | _n = -stepsToStop; // Start deceleration 246 | } 247 | else if (_n < 0) 248 | { 249 | // Currently decelerating, need to accel again? 250 | if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW) 251 | _n = -_n; // Start accceleration 252 | } 253 | } 254 | else if (distanceTo < 0) 255 | { 256 | // We are clockwise from the target 257 | // Need to go anticlockwise from here, maybe decelerate 258 | if (_n > 0) 259 | { 260 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 261 | if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW) 262 | _n = -stepsToStop; // Start deceleration 263 | } 264 | else if (_n < 0) 265 | { 266 | // Currently decelerating, need to accel again? 267 | if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW) 268 | _n = -_n; // Start accceleration 269 | } 270 | } 271 | 272 | // Need to accelerate or decelerate 273 | if (_n == 0) 274 | { 275 | // First step from stopped 276 | _cn = _c0; 277 | _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; 278 | } 279 | else 280 | { 281 | // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). 282 | _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 283 | _cn = max(_cn, _cmin); 284 | } 285 | _n++; 286 | _stepInterval = _cn; 287 | _speed = 1000000.0 / _cn; 288 | if (_direction == DIRECTION_CCW) 289 | _speed = -_speed; 290 | 291 | } 292 | 293 | // Run the motor to implement speed and acceleration in order to proceed to the target position 294 | // You must call this at least once per step, preferably in your main loop 295 | // If the motor is in the desired position, the cost is very small 296 | // returns true if the motor is still running to the target position. 297 | bool SStepper::run() 298 | { 299 | if (runSpeed()) 300 | computeNewSpeed(); 301 | return _speed != 0.0 || distanceToGo() != 0; 302 | } 303 | 304 | 305 | SStepper::SStepper( uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4) 306 | { 307 | _currentPos = 0; 308 | _targetPos = 0; 309 | _speed = 0.0; 310 | _maxSpeed = 1000.0; 311 | _acceleration = 0.0; 312 | _sqrt_twoa = 1.0; 313 | _stepInterval = 0; 314 | _minPulseWidth = 1; 315 | _enablePin = 0xff; 316 | _lastStepTime = 0; 317 | _pin[0] = pin1; 318 | _pin[1] = pin2; 319 | _pin[2] = pin3; 320 | _pin[3] = pin4; 321 | 322 | // NEW 323 | _n = 0; 324 | _c0 = 0.0; 325 | _cn = 0.0; 326 | _cmin = 1.0; 327 | _direction = DIRECTION_CCW; 328 | 329 | int i; 330 | for (i = 0; i < 4; i++) 331 | _pinInverted[i] = 0; 332 | enableOutputs(); 333 | } 334 | 335 | 336 | void SStepper::setMaxSpeed(float speed) 337 | { 338 | if (speed < 0.0) 339 | speed = -speed; 340 | if (_maxSpeed != speed) 341 | { 342 | _maxSpeed = speed; 343 | _cmin = 1000000.0 / speed; 344 | // Recompute _n from current speed and adjust speed if accelerating or cruising 345 | if (_n > 0) 346 | { 347 | _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 348 | computeNewSpeed(); 349 | } 350 | } 351 | } 352 | 353 | float SStepper::maxSpeed() 354 | { 355 | return _maxSpeed; 356 | } 357 | 358 | void SStepper::setSpeed(float speed) 359 | { 360 | if (speed == _speed) 361 | return; 362 | speed = constrain(speed, -_maxSpeed, _maxSpeed); 363 | if (speed == 0.0) 364 | _stepInterval = 0; 365 | else{ 366 | _stepInterval = fabs(1000000.0 / speed); 367 | _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW; 368 | } 369 | _speed = speed; 370 | } 371 | 372 | float SStepper::speed() 373 | { 374 | return _speed; 375 | } 376 | 377 | void SStepper::step(long step) 378 | { 379 | switch (step & 0x7) 380 | { 381 | case 0: // 1000 382 | setOutputPins(0b0001); 383 | break; 384 | 385 | case 1: // 1010 386 | setOutputPins(0b0101); 387 | break; 388 | 389 | case 2: // 0010 390 | setOutputPins(0b0100); 391 | break; 392 | 393 | case 3: // 0110 394 | setOutputPins(0b0110); 395 | break; 396 | 397 | case 4: // 0100 398 | setOutputPins(0b0010); 399 | break; 400 | 401 | case 5: //0101 402 | setOutputPins(0b1010); 403 | break; 404 | 405 | case 6: // 0001 406 | setOutputPins(0b1000); 407 | break; 408 | 409 | case 7: //1001 410 | setOutputPins(0b1001); 411 | break; 412 | } 413 | } 414 | 415 | // You might want to override this to implement eg serial output 416 | // bit 0 of the mask corresponds to _pin[0] 417 | // bit 1 of the mask corresponds to _pin[1] 418 | // .... 419 | void SStepper::setOutputPins(uint8_t mask) 420 | { 421 | uint8_t numpins = 4; 422 | uint8_t i; 423 | for (i = 0; i < numpins; i++) 424 | digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i])); 425 | } 426 | 427 | 428 | 429 | 430 | // Prevents power consumption on the outputs 431 | void SStepper::disableOutputs() 432 | { 433 | setOutputPins(0); // Handles inversion automatically 434 | if (_enablePin != 0xff) 435 | { 436 | pinMode(_enablePin, OUTPUT); 437 | digitalWrite(_enablePin, LOW ^ _enableInverted); 438 | } 439 | } 440 | 441 | void SStepper::enableOutputs() 442 | { 443 | pinMode(_pin[0], OUTPUT); 444 | pinMode(_pin[1], OUTPUT); 445 | pinMode(_pin[2], OUTPUT); 446 | pinMode(_pin[3], OUTPUT); 447 | 448 | 449 | if (_enablePin != 0xff) 450 | { 451 | pinMode(_enablePin, OUTPUT); 452 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 453 | } 454 | } 455 | 456 | void SStepper::setMinPulseWidth(unsigned int minWidth) 457 | { 458 | _minPulseWidth = minWidth; 459 | } 460 | 461 | void SStepper::setEnablePin(uint8_t enablePin) 462 | { 463 | _enablePin = enablePin; 464 | 465 | // This happens after construction, so init pin now. 466 | if (_enablePin != 0xff) 467 | { 468 | pinMode(_enablePin, OUTPUT); 469 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 470 | } 471 | } 472 | 473 | void SStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert) 474 | { 475 | _pinInverted[0] = stepInvert; 476 | _pinInverted[1] = directionInvert; 477 | _enableInverted = enableInvert; 478 | } 479 | 480 | void SStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 481 | { 482 | _pinInverted[0] = pin1Invert; 483 | _pinInverted[1] = pin2Invert; 484 | _pinInverted[2] = pin3Invert; 485 | _pinInverted[3] = pin4Invert; 486 | _enableInverted = enableInvert; 487 | } 488 | 489 | // Blocks until the target position is reached and stopped 490 | void SStepper::runToPosition() 491 | { 492 | while (run()) 493 | ; 494 | } 495 | 496 | bool SStepper::runSpeedToPosition() 497 | { 498 | if (_targetPos == _currentPos) 499 | return false; 500 | if (_targetPos >_currentPos) 501 | _direction = DIRECTION_CW; 502 | else 503 | _direction = DIRECTION_CCW; 504 | return runSpeed(); 505 | } 506 | 507 | // Blocks until the new target position is reached 508 | void SStepper::runToNewPosition(long position) 509 | { 510 | moveTo(position); 511 | runToPosition(); 512 | } 513 | 514 | void SStepper::stop() 515 | { 516 | if (_speed != 0.0) 517 | { 518 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding) 519 | if (_speed > 0) 520 | move(stepsToStop); 521 | else 522 | move(-stepsToStop); 523 | } 524 | } 525 | 526 | bool SStepper::isRunning() 527 | { 528 | return !(_speed == 0.0 && _targetPos == _currentPos); 529 | } 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | //Fin de la class SStepper 538 | --------------------------------------------------------------------------------