From 06a572af56be98d54f154cf255a4c234c45ec96b Mon Sep 17 00:00:00 2001 From: "yorick.geoffre" Date: Sun, 23 Apr 2023 22:11:21 +0200 Subject: [PATCH] various improvements --- .theia/launch.json | 12 +++- piconaut-caaar.ino | 136 +++++++++++++++++++++++++++++++++++---------- 2 files changed, 119 insertions(+), 29 deletions(-) diff --git a/.theia/launch.json b/.theia/launch.json index 7e4253b..507feec 100644 --- a/.theia/launch.json +++ b/.theia/launch.json @@ -3,6 +3,16 @@ // Hover to view descriptions of existing attributes. "version": "0.2.0", "configurations": [ - + { + "cwd": "${workspaceFolder}", + "executable": "./bin/executable.elf", + "name": "Debug with JLink", + "request": "launch", + "type": "cortex-debug", + "device": "", + "runToEntryPoint": "main", + "showDevDebugOutput": "none", + "servertype": "jlink" + } ] } diff --git a/piconaut-caaar.ino b/piconaut-caaar.ino index 9d9ba2e..81c5a08 100644 --- a/piconaut-caaar.ino +++ b/piconaut-caaar.ino @@ -1,29 +1,43 @@ -#include +#include +#define MAXIMUM_NUM_NEOPIXELS 7 #include const int numPins = 3; const int pwmPins[] = {A1, A2, A3}; const float minDutyCycle[numPins] = {5.98, 6.03, 6.03}; const float maxDutyCycle[numPins] = {12.08, 12.02, 11.94}; -const float deadzone = 0.1; +const float deadzone = 0.50; volatile float mappedDutyCycles[numPins] = {0.0, 0.0, 0.0}; +enum devMode{ + automatic = 0, + safe = 1, + manual = 2 +}; + +volatile devMode mode = safe; + // WS2812B LEDs -const int numLEDs = 6; -const int ledPin = 10; -Adafruit_NeoPixel strip(numLEDs, ledPin, NEO_GRB + NEO_KHZ800); +const int numLEDs = 7; +const int ledPin = 15; +NeoPixelConnect p(ledPin, numLEDs, pio1, 0); // Servo -const int servoPins[] = {11, 12}; +const int servoPins[] = {10, 6}; const int numServos = 2; const int servoFreq = 60; RP2040_PWM* servo1; RP2040_PWM* servo2; +float servo1Perc = 0.0; +float servo2Perc = 0.0; +//system float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } +void runDirectMode(); + void setup() { Serial.begin(9600); delay(5000); @@ -33,16 +47,37 @@ void setup() { } // Set up WS2812B LEDs - strip.begin(); - strip.show(); - setLEDDefaultColors(); + //strip.begin(); + //strip.show(); + //setLEDDefaultColors(); // Set up servo PWM for (int i = 0; i < numServos; i++) { pinMode(servoPins[i], OUTPUT); } - servo1 = new RP2040_PWM(servoPins[0], servoFreq, 0); - servo2 = new RP2040_PWM(servoPins[1], servoFreq, 0); + servo1 = new RP2040_PWM(servoPins[0], servoFreq, 0); + servo2 = new RP2040_PWM(servoPins[1], servoFreq, 0); + + servo1->setPWM(); + servo2->setPWM(); +} + +void runLEDs(){ + switch((int)mode){ + case 0: + p.neoPixelFill(0, 0, 255, true); + break; + case 1: + p.neoPixelFill(255, 0, 0, true); + break; + case 2: + setLEDDefaultColors(); + break; + default: + p.neoPixelFill(0, 255, 0, true); + Serial.println(mode); + break; + } } void loop() { @@ -54,7 +89,9 @@ void loop() { // Map the duty cycle value to the range of -1.0 to 1.0 mappedDutyCycles[i] = mapFloat(dutyCycle, minDutyCycle[i], maxDutyCycle[i], -1.0, 1.0); - + Serial.print(i); + Serial.print(" "); + Serial.println(period); // Apply deadzone if (mappedDutyCycles[i] > -deadzone && mappedDutyCycles[i] < deadzone) { mappedDutyCycles[i] = 0.0; @@ -70,35 +107,53 @@ void setup1() { } void loop1() { + modeCheck(); + runLEDs(); + switch(mode){ + case automatic://do nothing, values prodivded by host computer + break; + case safe: //stop car + servo1Perc = 0.0f; + servo2Perc = 0.0f; + break; + case manual: + servo1Perc = ((mappedDutyCycles[1] + 1.0) * 3); + servo2Perc = ((mappedDutyCycles[2] + 1.0) * 3); + //Serial.println(servo1Perc); + //Serial.println(servo1Perc); + break; + } + updateServos(); handleSerialCommands(); // Print the pin name and the mapped duty cycle in a format suitable for the Serial Plotter - for (int i = 0; i < numPins; i++) { - Serial.print("Pin "); + /*for (int i = 0; i < numPins; i++) { Serial.print(pwmPins[i]); - Serial.print(": "); + Serial.print(":"); Serial.print(mappedDutyCycles[i]); if (i < numPins - 1) { - Serial.print(", "); + Serial.print(","); } else { Serial.println(); } - } + }*/ delay(1); // Wait for 1ms before the next transmission } void setLEDDefaultColors() { - for (int i = 0; i < 4; i++) { - strip.setPixelColor(i, strip.Color(255, 255, 255)); + for (int i = 0; i < 2; i++) { + //strip.setPixelColor(i, strip.Color(255, 0, 0)); + p.neoPixelSetValue(i, 255,0,0, true); } - for (int i = 4; i < 6; i++) { - strip.setPixelColor(i, strip.Color(255, 0, 0)); + for (int i = 2; i < 6; i++) { + //strip.setPixelColor(i, strip.Color(255, 255, 255)); + p.neoPixelSetValue(i, 255,255,255, true); } - strip.show(); + //strip.show(); } -void cycleLEDColors() { +void cycleLEDColors() {/* static uint32_t color = strip.Color(255, 0, 0); static uint8_t colorState = 0; @@ -115,7 +170,8 @@ void cycleLEDColors() { } else { color = strip.Color(255, 0, 0); } - } + }*/ + p.neoPixelFill(255, 0, 0, true); } void handleSerialCommands() { @@ -127,16 +183,16 @@ void handleSerialCommands() { } if (command.startsWith("l")) { if (command == "l1") { - setLEDDefaultColors(); + //setLEDDefaultColors(); } else if (command == "l0") { - cycleLEDColors(); + //cycleLEDColors(); } else if (command.startsWith("l")) { int ledNum; int r, g, b; if (sscanf(command.c_str(), "l%d %d,%d,%d", &ledNum, &r, &g, &b) == 4) { if (ledNum >= 0 && ledNum < numLEDs) { - strip.setPixelColor(ledNum, strip.Color(r, g, b)); - strip.show(); + //strip.setPixelColor(ledNum, strip.Color(r, g, b)); + //strip.show(); } } } @@ -152,3 +208,27 @@ void handleSerialCommands() { } } } + +void modeCheck(){ + for (int i = 0; i < 3; i++) { + if (mappedDutyCycles[i] < -1.3 || mappedDutyCycles[i] > 1.3) { + mode = safe; //out of bounds/disconnected + Serial.println("#OUT_OF_BOUNDS"); + return; + } + } + + if(mappedDutyCycles[0] > 0.5){ //interrupteur sur le côté de la télécommande + mode = manual; + }else if(mappedDutyCycles[0] > -0.25 && mappedDutyCycles[0] < 0.25){ + mode = automatic; + } + else if(mappedDutyCycles[0] < -0.5){ + mode = safe; + } +} + +void updateServos(){ + servo1->setPWM(servoPins[0],60,servo1Perc+5.5f); //accelerateur + servo2->setPWM(servoPins[1],60,servo2Perc); //dir +} \ No newline at end of file