diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..8708667 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,19 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nano] +platform = atmelavr +board = nanoatmega328 +framework = arduino +lib_deps = + waspinator/AccelStepper@^1.64 + arduino-libraries/Servo@^1.1.8 + adafruit/Adafruit MPR121@^1.1.1 + marcoschwartz/LiquidCrystal_I2C@1.1.2 diff --git a/src/charset.h b/src/charset.h new file mode 100644 index 0000000..1eb9442 --- /dev/null +++ b/src/charset.h @@ -0,0 +1,86 @@ +uint8_t up[8] = { + 0b00000, + 0b00100, + 0b01110, + 0b10101, + 0b00100, + 0b00100, + 0b00100, + 0b00000 +}; + +uint8_t down[8] = { + 0b00000, + 0b00100, + 0b00100, + 0b00100, + 0b10101, + 0b01110, + 0b00100, + 0b00000 +}; + +uint8_t bullet[8] = { + 0b00000, + 0b00100, + 0b00010, + 0b11111, + 0b00010, + 0b00100, + 0b00000, + 0b00000 +}; + +uint8_t right[8] = { + 0b00000, + 0b00100, + 0b00010, + 0b11111, + 0b10010, + 0b10100, + 0b10000, + 0b00000 +}; + +uint8_t left[8] = { + 0b00000, + 0b00100, + 0b01000, + 0b11111, + 0b01001, + 0b00101, + 0b00001, + 0b00000 +}; +uint8_t pause[8] = { + 0b00000, + 0b00000, + 0b01010, + 0b01010, + 0b01010, + 0b01010, + 0b00000, + 0b00000 +}; + +uint8_t agrave[8] = { + 0b01000, + 0b00100, + 0b01110, + 0b00001, + 0b01111, + 0b10001, + 0b01111, + 0b00000 +}; + +uint8_t ecirc[8] = { + 0b00100, + 0b01010, + 0b01110, + 0b10001, + 0b11111, + 0b10001, + 0b01110, + 0b00000 +}; \ No newline at end of file diff --git a/src/my_buttons.cpp b/src/my_buttons.cpp new file mode 100644 index 0000000..0322b52 --- /dev/null +++ b/src/my_buttons.cpp @@ -0,0 +1,163 @@ +#include +// #include "my_buttons.h" +#include "Adafruit_MPR121.h" + +#ifndef _BV +#define _BV(bit) (1 << (bit)) +#endif + +int LONG_CLICK_DELAY = 500; +int DEBOUNCING_DELAY = 300; +int lastButtonId = -1; +unsigned long lastClick = 0; +unsigned long lastRawPressed = 0; + +Adafruit_MPR121 cap = Adafruit_MPR121(); +// Keeps track of the last pins touched +// so we know when buttons are 'released' +uint16_t lasttouched = 0; +uint16_t currtouched = 0; + +void setupButtons() +{ +if (!cap.begin(0x5A)) { + Serial.println("MPR121 not found, check wiring?"); + while (1); + } + Serial.println("MPR121 found!"); +} + + + + +int getPressedButton() { + +int b = -1; +// Get the currently touched pads + currtouched = cap.touched(); + + for (uint8_t i=0; i<12; i++) { + // it if *is* touched + if (currtouched & _BV(i)){ + b = i; + lastRawPressed = millis(); + } + if (b>= 0) break ; + } + + if (b >= 0){ + + unsigned long currentClick = millis(); + if (lastButtonId != b) { + if (currentClick > lastClick + DEBOUNCING_DELAY) { + Serial.println(" Sort"); + lastClick = currentClick; + tone(12, 1000, 50); + lastButtonId = b; + } else { + b = -1; + } + } else { + if (currentClick > lastClick + LONG_CLICK_DELAY) { + Serial.println(" Long"); + lastButtonId = b; + b = b + 12; + lastClick = currentClick; + } else { + b = -1; + } + } + } else { + if (millis() - lastRawPressed > 100) { + lastButtonId = -1; + } + } + return b; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + +/**** + + + + + int raw = analogRead(KEYS_PIN); + int b = -1; + if (raw >= 472) { + lastRawPressed = millis(); + if (raw >= 981) { + b = 1; + } else if (raw >= 894 && raw <= 966) { + b = 2; + } else if (raw >= 823 && raw < 877) { + b = 3; + } else if (raw >= 763 && raw <= 817) { + b = 4; + } else if (raw >= 707 && raw <= 753) { + b = 5; + } else if (raw >= 662 && raw <= 698) { + b = 6; + } else if (raw >= 622 && raw <= 698) { + b = 7; + } else if (raw >= 586 && raw <= 614) { + b = 8; + } else if (raw >= 556 && raw <= 584) { + b = 9; + } else if (raw >= 526 && raw <= 554) { + b = 10; + } else if (raw >= 501 && raw <= 519) { + b = 0; + } else if (raw >= 472 && raw <= 508) { + b = 11; + } + unsigned long currentClick = millis(); + if (lastButtonId != b) { + if (currentClick > lastClick + DEBOUNCING_DELAY) { + lastClick = currentClick; + lastButtonId = b; + } else { + b = -1; + } + } else { + if (currentClick > lastClick + LONG_CLICK_DELAY) { + lastButtonId = b; + b = b + 12; + lastClick = currentClick; + } else { + b = -1; + } + } + } else { + if (millis() - lastRawPressed > 100) { + lastButtonId = -1; + } + } + return b; +} + +***/ diff --git a/src/my_buttons.h b/src/my_buttons.h new file mode 100644 index 0000000..b5ec765 --- /dev/null +++ b/src/my_buttons.h @@ -0,0 +1,6 @@ + + +#define KEYS_PIN A0 + +int getPressedButton(); +void setupButtons(); diff --git a/src/oorobot.ino b/src/oorobot.ino new file mode 100644 index 0000000..58cdc52 --- /dev/null +++ b/src/oorobot.ino @@ -0,0 +1,810 @@ +/* + OoRoBoT code + + Get the following libraries (using the Arduino IDE library manager) + - **AccelStepper** v1.57.1 by Mike McCauley + - **LiquidCrystal I2C** v1.1.2 by Frank de Brabander + +*/ +#include +#include +#include +#include +#include +#include "charset.h" +//#include "buttons.h" +#include "my_buttons.h" + + +#define OOROBOT_VERSION "1.1.2" + +#define SCREEN_TIMEOUT 25 + +// On some step motors direction may be inverted +// define this symbol to invert the motors direction +#define INVERT_DIRECTION +#define MAX_COMMANDS 512 +#define MAX_LOOPS 10 + +// define this symbol to add bluetooth support +// #define HAVE_BLUETOOTH + +#ifdef HAVE_BLUETOOTH +# include +# define RxD 12 +# define TxD 13 +SoftwareSerial BTSerie(RxD, TxD); +#endif + +// motor pins +#define motorPin1 4 // IN1 on the ULN2003 driver 1 +#define motorPin2 5 // IN2 on the ULN2003 driver 1 +#define motorPin3 6 // IN3 on the ULN2003 driver 1 +#define motorPin4 7 // IN4 on the ULN2003 driver 1 + +#define motorPin5 8 // IN1 on the ULN2003 driver 2 +#define motorPin6 9 // IN2 on the ULN2003 driver 2 +#define motorPin7 10 // IN3 on the ULN2003 driver 2 +#define motorPin8 11 // IN4 on the ULN2003 driver 2 + +// Menu index +#define START_MENU 0 +#define SETTINGS_MENU 1 +#define CTRL_MENU 2 +#define RUNNING_MENU 3 +#define OFF_MENU 4 + +#define MAX_STEPPER_SPEED 900 +#define MIN_STEPPER_SPEED 200 +#define WHEEL_SPACING_MM 132 + +int stepperSpeed = MIN_STEPPER_SPEED; + +// Initialize with pin sequence IN1-IN3-IN2-IN4 for using the AccelStepper with 28BYJ-48 +AccelStepper stepper1(AccelStepper::HALF4WIRE, motorPin1, motorPin3, motorPin2, motorPin4); +AccelStepper stepper2(AccelStepper::HALF4WIRE, motorPin5, motorPin7, motorPin6, motorPin8); + +Servo penServo; + +// The I2C LCD object +LiquidCrystal_I2C lcd(0x27, 16, 2); + +struct Params { + int stepCm; + int turnSteps; + int lineSteps; +}; + +int stepDelay = 800; +int steps1 = 0; // keep track of the step count for motor 1 +int steps2 = 0; // keep track of the step count for motor 2 +int isMoving = false; +/* +char buttonsMap[] = { + 'L', 'G', 's', 'C', 0, 'R', 0, 'U', 'P', 'D', '|', '!', + '-', 0, 'S', 'A', 0, '+', 0, 0, 0, 0, 0, 0 +}; +*/ +char buttonsMap[] = { + 'C', 0, 'D', '|', 's', 'R','P', 'L', 'G', 0, 'U', '!', + 'A', 0, 0, 0, 'S', '+' , 0, '-', 0, 0, 0, 0 +}; + +Params params = {140, 1430, 100}; +int previousMenu = CTRL_MENU; +int selectedMenu = START_MENU; +char cmd[MAX_COMMANDS + 1] = {}; +int loopCounter[MAX_LOOPS] = {}; +int loopPointer[MAX_LOOPS] = {}; +int loopIndex=0; +int currentRadius=0; +boolean reverseOrientation=false; + +unsigned char cmd_l = 0; +int changeDisplay = 1; +long lastChangeDisplay = 0; +unsigned int selectedLine = 0; +short commandLaunched = 0; +short consecutive_numbers = 0; +#define MAX_CONSECUTIVE_NUMBERS 3 +short num_of_cmd = 0; +short max_num_cmd = 0; +long startMovement=0; + + + +void moveServo(int angle) { + penServo.attach(3); + penServo.write(angle); + delay(200); + penServo.detach(); +} + + +void setup() { + Serial.begin(9600); + loadParams(); + setupButtons(); + Serial.println(F("Setup")); + + tone(12, 1047, 100); + delay(150); + tone(12, 1319, 100); + delay(150); + tone(12, 1560, 100); + delay(150); + noTone(12); + + lcd.init(); + lcd.backlight(); + lcd.createChar(1, up); + lcd.createChar(2, down); + lcd.createChar(4, right); + lcd.createChar(3, left); + lcd.createChar(5, pause); + lcd.createChar(6, bullet); + lcd.createChar(7, agrave); + + // stepper motors init + stepper1.setMaxSpeed(1000); + stepper1.move(1); + + stepper2.setMaxSpeed(1000); + stepper2.move(-1); + + moveServo(5); + + // Bluetooth module init +#ifdef HAVE_BLUETOOTH + pinMode(RxD, INPUT); + pinMode(TxD, OUTPUT); + BTSerie.begin(9600); +#endif +} + +void loop() { + long currentTime = millis(); + int buttonId = getPressedButton(); + char button = 0; + if (buttonId >= 0) { + button = buttonsMap[buttonId]; + if (button != 0) { + actionButtonForScreen(button); + } + } else { +#ifdef HAVE_BLUETOOTH + while (BTSerie.available()) { + selectedMenu = CTRL_MENU; + button = BTSerie.read(); + //Serial.println(button); + if (button != 0) { + actionButtonForScreen(button); + } + } +#endif + while (Serial.available()) { + selectedMenu = CTRL_MENU; + button = Serial.read(); + //Serial.println(button); + if (button != 0) { + actionButtonForScreen(button); + } + } + } + + if (isMoving) { + if (isCommandTerminated()) { + Serial.println(F("step delay")); + if (! launchNextCommand()) { + Serial.println(F("program terminated")); + isMoving = false; + disableMotors(); + lcd.clear(); + lcd.setBacklight(HIGH); + lcd.print(F("fin !")); +#ifdef HAVE_BLUETOOTH + BTSerie.println(F("fin !")); +#endif + delay(1000); + selectedMenu = CTRL_MENU; + changeDisplay = 1; + } + } + } + updateScreen(); + + // Screen off after 4s + if (currentTime > lastChangeDisplay + SCREEN_TIMEOUT * 1000) { + if (selectedMenu != OFF_MENU) { + previousMenu = selectedMenu; + selectedMenu = OFF_MENU; + changeDisplay = 1; + } + } + +} + + + +void actionButtonForScreen(char button) { + + if (selectedMenu == START_MENU) { + selectedMenu = CTRL_MENU; + changeDisplay = 1; + } else if (selectedMenu == CTRL_MENU) { + changeDisplay = 1; + Serial.print(F("New char : ")); + Serial.println(button); + if ( button > 47 && button < 58) // it's a number + { + consecutive_numbers++; + if (consecutive_numbers > MAX_CONSECUTIVE_NUMBERS) + { + Serial.println(F("number too hight")); + return; + } + else + { + cmd[cmd_l++] = button; + return; + } + } + else + { + switch (button) { + case 'S': + selectedMenu = SETTINGS_MENU; + changeDisplay = 1; + break; + case 'G': + stepDelay = 800; + selectedMenu = RUNNING_MENU; + isMoving = true; + commandLaunched = 0; + + max_num_cmd = num_of_cmd; + num_of_cmd = 0; + launchNextCommand(); + + break; + case 'A': + cmd_l = 0; + commandLaunched = 0; + num_of_cmd = 0; + break; + case 'C': + while (cmd_l > 0 && cmd[cmd_l - 1] > 47 && cmd[cmd_l - 1] < 58) + { + cmd_l--; //remove all number; + } + if (cmd_l > 0) { + cmd_l--; + num_of_cmd--; + } + break; + case 'B' : + case 'a' : + case 'c' : + case 'r' : + case 'E' : + case 'U' : + case 'D' : + case 'L' : + case 'R' : + case 'W' : + case '!' : + case '|' : + case 'P' : + if (cmd_l < MAX_COMMANDS) { + cmd[cmd_l++] = button; + num_of_cmd++; + } else { + Serial.println(F("too many commands")); + } + break; + case 0 : + case '+' : + case '-' : + case 's' : + changeDisplay = 0; + break; + default: + Serial.println(F("Unknown Command")); +#ifdef HAVE_BLUETOOTH + BTSerie.println(F("Unknown Command")); +#endif + break; + } + consecutive_numbers = 0; + } + } else if (selectedMenu == SETTINGS_MENU) { + actionButtonForSettingsScreen(button); + } else if (selectedMenu == RUNNING_MENU) { + disableMotors(); + isMoving = false; + lcd.clear(); + lcd.setBacklight(HIGH); + lcd.print(F("Arret!")); + delay(1000); + selectedMenu = CTRL_MENU; + changeDisplay = 1; + } else if (selectedMenu == OFF_MENU) { + selectedMenu = previousMenu; + changeDisplay = 1; + } +} + +void actionButtonForSettingsScreen(char button) { + changeDisplay = 1; + switch (button) { + case 'U': + selectedLine--; + selectedLine = selectedLine % 3; + break; + case 'D': + selectedLine++; + selectedLine = selectedLine % 3; + break; + case 'R': + if (selectedLine == 0) { + params.stepCm++; + } else if (selectedLine == 1) { + params.turnSteps++; + } else { + params.lineSteps++; + } + break; + case '+': + if (selectedLine == 0) { + params.stepCm += 10; + } else if (selectedLine == 1) { + params.turnSteps += 10; + } else { + params.lineSteps += 10; + } + break; + case 'L': + if (selectedLine == 0) { + params.stepCm--; + } else if (selectedLine == 1) { + params.turnSteps--; + } else { + params.lineSteps--; + } + break; + case '-': + if (selectedLine == 0) { + params.stepCm -= 10; + } else if (selectedLine == 1) { + params.turnSteps -= 10; + } else { + params.lineSteps -= 10; + } + break; + case 's': + case 'G': + saveParams(); + selectedMenu = CTRL_MENU; + break; + case 'C': + loadParams(); + selectedMenu = CTRL_MENU; + break; + } +} + +void updateScreen() { + if (changeDisplay) { + if (selectedMenu == START_MENU) { + //lcd.setBacklight(HIGH); + lcd.display(); + lcd.setCursor(0, 0); + lcd.print(" OoRoBoT " OOROBOT_VERSION); + lcd.setCursor(0, 1); + lcd.print(F("Pret \7 demarrer!")); +#ifdef HAVE_BLUETOOTH + BTSerie.println("OoRoBoT " OOROBOT_VERSION); + BTSerie.println(F("En attente de commandes")); +#endif + previousMenu = CTRL_MENU; + selectedMenu = CTRL_MENU; + } else if (selectedMenu == CTRL_MENU) { + lcd.setBacklight(HIGH); + cmd[cmd_l] = 0; +#ifdef HAVE_BLUETOOTH + BTSerie.println(cmd); +#endif + lcd.clear(); + lcd.setCursor(0, 0); + for (char i = 0 ; i < cmd_l ; i++) { + if (i == 16) { + lcd.setCursor(0, 1); + } + lcd.print(commandToDisplay(cmd[i])); + } + } else if (selectedMenu == SETTINGS_MENU) { + lcd.setBacklight(HIGH); + lcd.clear(); + int cm = params.stepCm / 10; + int mm = params.stepCm - 10 * cm; + int currentLine=selectedLine; + int stepIdx=0; + int turnIdx=1; + int lineIdx=2; + Serial.print("selectedLine:"); + Serial.println(selectedLine); + if (selectedLine==2) { + stepIdx=2; + turnIdx=2; + lineIdx=0; + } + if (stepIdx<2) { + lcd.setCursor(0, stepIdx); + lcd.print(F(" Distance:")); + lcd.print(cm); + lcd.print(F(".")); + lcd.print(mm); + lcd.print(F("cm")); + } + if (turnIdx<2) { + lcd.setCursor(0, turnIdx); + lcd.print(F(" 1/4Tour:")); + lcd.print(params.turnSteps); + lcd.print(F("pas")); + } + if (lineIdx<2) { + lcd.setCursor(0, lineIdx); + lcd.print(F(" 1cm:")); + lcd.print(params.lineSteps); + lcd.print(F("pas")); + } + + lcd.setCursor(0, selectedLine%2); + lcd.print("\6"); +#ifdef HAVE_BLUETOOTH + if (selectedLine == 0) { + BTSerie.print(F("Distance:")); + BTSerie.print(cm); + BTSerie.print(F(".")); + BTSerie.print(mm); + BTSerie.println(F("cm")); + } else { + BTSerie.print(F("1/4Tour:")); + BTSerie.print(params.turnSteps); + BTSerie.println(F("pas")); + } +#endif + + } else if (selectedMenu == CTRL_MENU) { + lcd.setBacklight(HIGH); + } else if (selectedMenu == OFF_MENU) { + lcd.setBacklight(LOW); + } + lastChangeDisplay = millis(); + } + changeDisplay = 0; +} + +char commandToDisplay(char c) { + switch (c) { + case 'U': + return 1; + break; + case 'D': + return 2; + break; + case 'L': + return 3; + break; + case 'R': + return 4; + break; + case 'P': + return 5; + break; + default: + return c; + } +} + +int startLoop=-1; +int remainingLoop=0; + +boolean launchNextCommand() { + if (commandLaunched >= cmd_l) { + return false; + } else { + disableMotors(); + lcd.clear(); + if (stepDelay>100) { + lcd.setBacklight(HIGH); + lcd.print((num_of_cmd + 1)); + lcd.print(F(" sur ")); + lcd.print(max_num_cmd); + lcd.print(F(" : ")); + lcd.print(commandToDisplay(cmd[commandLaunched])); + } +#ifdef HAVE_BLUETOOTH + BTSerie.println(cmd[commandLaunched]); +#endif + Serial.print(F("etape ")); + Serial.print((num_of_cmd + 1)); + Serial.print(F(" sur ")); + Serial.print(max_num_cmd); + Serial.print(F(" : ")); + Serial.println(cmd[commandLaunched]); + + num_of_cmd++; + delay(stepDelay); + // lcd.setBacklight(LOW); + + enableMotors(); + + char command = cmd[commandLaunched]; + short stepSize = getStepSize(cmd, &commandLaunched); + + Serial.print(F("stepSize :")); + Serial.println(stepSize); + switch (command) { + case 'W': + Serial.println(F("set waiting step delay")); + stepDelay=stepSize; + break; + case 'U': + Serial.println(F("stepForward")); + stepForward(stepSize); + break; + case 'D': + Serial.println(F("stepBackward")); + stepBackward(stepSize); + break; + case 'L': + Serial.println(F("turnLeft")); + turnLeft(stepSize); + break; + case 'R': + Serial.println(F("turnRight")); + turnRight(stepSize); + break; + case 'P': + Serial.println(F("pause")); + delay(stepSize); + break; + case '!' : + Serial.println(F("pen down")); + moveServo(0); + break; + case '|' : + Serial.println(F("pen up")); + moveServo(30); + break; + case 'c' : + currentRadius = stepSize; + break; + case 'r': + if (stepSize==1) { + reverseOrientation=true; + } else { + reverseOrientation=false; + } + break; + case 'a' : + Serial.println(F("doCircle")); + doCircle(currentRadius, stepSize, reverseOrientation); + break; + case 'B': + Serial.println(F("begin loop")); + if (loopIndex>=MAX_LOOPS) { + Serial.println(F("too many loops included")); + } else { + loopCounter[loopIndex] = stepSize; + loopPointer[loopIndex] = commandLaunched; + loopIndex++; + startLoop=commandLaunched; + remainingLoop=stepSize; + } + break; + case 'E': + Serial.println(F("end loop")); + if (loopCounter[loopIndex-1]>1) { + commandLaunched=loopPointer[loopIndex-1]; + loopCounter[loopIndex-1]--; + } else { + if (loopIndex>0) { + loopIndex--; + } + } + break; + } + commandLaunched++; + return true; + } +} + +short getStepSize(char* cmd, short* commandLaunched) +{ + char command = cmd[*commandLaunched]; + int stepsize = 0; + for (short i = 0; i < MAX_CONSECUTIVE_NUMBERS ; i++) + { + if (*commandLaunched + 1 < MAX_COMMANDS - 1 ) + { + if (cmd[*commandLaunched + 1] > 47 && cmd[*commandLaunched + 1] < 58) + { + stepsize = stepsize * 10 + cmd[*commandLaunched + 1 ] - 48; + *commandLaunched = *commandLaunched + 1; + } + } + } + + if (stepsize == 0) + { + switch (command) { + case 'W': + stepsize = stepDelay; + break; + case 'U': + case 'D': + stepsize = params.stepCm; //10cm + break; + case 'L': + case 'R': + stepsize = 90; // 90°; + break; + case 'P': + stepsize=stepDelay*2; + break; + } + } + + return stepsize; + +} + +boolean isCommandTerminated() { + /* + int diff = (millis() - startMovement); + if (diff>=100) { + startMovement=millis(); + stepperSpeed+=100; + if (stepperSpeed>MAX_STEPPER_SPEED) { + stepperSpeed=MAX_STEPPER_SPEED; + } else { + Serial.print(F("speed:")); + Serial.println(stepperSpeed); + stepper2.setSpeed(stepperSpeed); + stepper1.setSpeed(stepperSpeed); + } + } + */ + stepper2.setSpeed(MAX_STEPPER_SPEED); + stepper1.setSpeed(MAX_STEPPER_SPEED); + + steps1 = stepper1.distanceToGo(); + steps2 = stepper2.distanceToGo(); + //Serial.println(steps1); + stepper1.runSpeedToPosition(); + stepper2.runSpeedToPosition(); + + if (steps1 == 0 && steps2 == 0) { + return true; + } else { + return false; + } +} + +void enableMotors() { + stepper1.setCurrentPosition(0); + stepper2.setCurrentPosition(0); + stepper1.enableOutputs(); + stepper2.enableOutputs(); +} + +void disableMotors() { + stepper1.stop(); + stepper2.stop(); + stepper1.disableOutputs(); + stepper2.disableOutputs(); +} + +void doCircle(float radius, float angle, boolean reverseOrientation){ + isMoving = true; + startMovement=millis(); + //AAW10r1c50a90G + //AAW10r0c200a45G + float lenght_big_arc, lenght_small_arc; + float steps_big_arc, steps_small_arc; + float speed_big_arc, speed_small_arc; + + lenght_big_arc = 2 * PI * (radius + (WHEEL_SPACING_MM/2)) * (angle / 360); + steps_big_arc = round(lenght_big_arc * params.stepCm / 10); + speed_big_arc = MAX_STEPPER_SPEED; + + lenght_small_arc = 2 * PI * (radius - (WHEEL_SPACING_MM/2)) * (angle / 360); + steps_small_arc = round(lenght_small_arc * params.stepCm / 10); + speed_small_arc = abs((steps_small_arc*speed_big_arc)/steps_big_arc); + + if (reverseOrientation) { + stepper2.move(-steps_big_arc); + stepper2.setSpeed(speed_big_arc); + stepper1.move(-steps_small_arc); + stepper1.setSpeed(speed_small_arc); + } else { + stepper1.move(steps_big_arc); + stepper1.setSpeed(speed_big_arc); + stepper2.move(steps_small_arc); + stepper2.setSpeed(speed_small_arc); + } + +} + +void stepForward(short distance) { + isMoving = true; + startMovement=millis(); + stepperSpeed = MIN_STEPPER_SPEED; + + int target = (int) (((float)params.stepCm / 10.0f) * ((float)distance * (float)params.lineSteps / 100.0f)); +#ifdef INVERT_DIRECTION + target = target * -1; +#endif + + stepper1.move(-target); + stepper2.move(target); +} + +void stepBackward(short distance) { + isMoving = true; + startMovement=millis(); + stepperSpeed = MIN_STEPPER_SPEED; + + int target = (int) (((float)params.stepCm / 10.0f) * ((float)distance * (float)params.lineSteps / 100.0f)); +#ifdef INVERT_DIRECTION + target = target * -1; +#endif + stepper1.moveTo(target); + stepper2.moveTo(-target); +} + +void turnLeft(short angle) { + isMoving = true; + startMovement=millis(); + stepperSpeed = MIN_STEPPER_SPEED; + + int angleStep = (int)((float)angle / 90.0 * (float)params.turnSteps); + stepper1.moveTo(angleStep); + stepper2.moveTo(angleStep); +} + +void turnRight(short angle) { + isMoving = true; + startMovement=millis(); + stepperSpeed = MIN_STEPPER_SPEED; + + int angleStep = (int)((float)angle / 90.0 * (float)params.turnSteps); + stepper1.moveTo(-angleStep); + stepper2.moveTo(-angleStep); +} + +void saveParams() { + EEPROM.put(0, params); +} + +void loadParams() { + Params savedParams; + EEPROM.get(0, savedParams); + if (savedParams.stepCm > 0 && savedParams.stepCm < 500) { + params.stepCm = savedParams.stepCm; + } else { + params.stepCm = 140; + } + if (savedParams.turnSteps > 0 && savedParams.turnSteps < 5000) { + params.turnSteps = savedParams.turnSteps; + } else { + params.turnSteps=1250; + } + if (savedParams.lineSteps > 0 && savedParams.lineSteps < 500) { + params.lineSteps = savedParams.lineSteps; + } else { + params.lineSteps = 140; + } +} diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html