From 23ff9e1c83d566050a7ebc40eab52c0d61f7b72d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Wed, 9 May 2018 10:42:10 +0200 Subject: [PATCH 01/28] Cleaning up Battery library - Changed all references to State Of Charge (SOC) to be called Voltage SOC is measured in %-units and depend on the discharge characteristics of the battery. Calculating it is no simple task. What's measured and presented by the functions is the battery voltage, and their names should reflect this. - Made all functions handle battery voltate in units of mV Before, it was handled in units of 10 mV/0.01 V. mV makes more sense. - Remove definition of battery types from Battery.h Duplicates of definitions in Definition.h - Reorder declarations and definitions of functions so that they match - Tweaking grouping of related functions - Commenting functions and algorithms --- Liam/Battery.cpp | 73 ++++++++++++++++++++++++++------------------- Liam/Battery.h | 38 +++++++++++------------ Liam/Definition.cpp | 2 +- Liam/Definition.h | 8 ++--- Liam/Liam.ino | 16 +++++----- Liam/MyDisplay.cpp | 6 ++-- Liam/SetupDebug.cpp | 10 +++---- 7 files changed, 82 insertions(+), 71 deletions(-) diff --git a/Liam/Battery.cpp b/Liam/Battery.cpp index e6f8743..95d27b5 100644 --- a/Liam/Battery.cpp +++ b/Liam/Battery.cpp @@ -9,13 +9,17 @@ */ #include "Battery.h" +#include "Definition.h" -/** Specific constructor. - */ -BATTERY::BATTERY(int type, int socpin, int dockpin) { +BATTERY::BATTERY(int type, int sensepin, int dockpin) { batType = type; - batSocpin = socpin; - batDockpin = dockpin; + batSensePin = sensepin; + batDockPin = dockpin; + + // Battery types are defined in Definition.h + // LIION + // NIMH + // LEAD_ACID if (batType == LIION) { fullyChargedLevel = LIIONFULL; @@ -35,22 +39,22 @@ BATTERY::BATTERY(int type, int socpin, int dockpin) { } -// set the level when battery is concidered fully charged -void BATTERY::setFullyChargedLevel(int level) { - fullyChargedLevel = level; -} - int BATTERY::getBatteryType() { return batType; } +// Set the voltage at which battery is considered fully charged (mV) +void BATTERY::setFullyChargedLevel(int level) { + fullyChargedLevel = level; +} + int BATTERY::getFullyChargedLevel() { return fullyChargedLevel; } -// set the level when battery is concidered depleted +// Set the voltage at which battery is considered depleted (mV) void BATTERY::setDepletedLevel(int level) { depletedLevel = level; } @@ -59,36 +63,45 @@ int BATTERY::getDepletedLevel() { return depletedLevel; } -int BATTERY::getSOC() { - return averageSOC; + +bool BATTERY::mustCharge() { + return (averageVoltage < depletedLevel); } -void BATTERY::resetSOC() { - averageSOC = readBatteryAndCalcValue(); +bool BATTERY::isBeingCharged() { + return digitalRead(batDockPin); } -bool BATTERY::mustCharge() { - return (averageSOC < depletedLevel); +bool BATTERY::isFullyCharged() { + return (readBatteryAndCalcValue() > fullyChargedLevel); } -void BATTERY::updateSOC() { - averageSOC = averageSOC - (averageSOC / FILTER) + (readBatteryAndCalcValue() / FILTER); +// Get battery voltage in mV (filtered through running average) +int BATTERY::getVoltage() { + return averageVoltage; } - -word BATTERY::readBatteryAndCalcValue(){ - unsigned long newReading = analogRead(batSocpin); - newReading = newReading * 488 * VOLTDIVATOR; - newReading /= 10000; - //return newReading; - return word(newReading); +void BATTERY::resetVoltage() { + averageVoltage = readBatteryAndCalcValue(); } -bool BATTERY::isBeingCharged() { - return digitalRead(batDockpin); +// Take a battery reading and recalculate running average +void BATTERY::updateVoltage() { + averageVoltage -= averageVoltage / FILTER; + averageVoltage += readBatteryAndCalcValue() / FILTER; } -bool BATTERY::isFullyCharged() { - return (readBatteryAndCalcValue() > fullyChargedLevel); +// Measure battery voltage in mV +word BATTERY::readBatteryAndCalcValue(){ + unsigned long reading = analogRead(batSensePin); + + // Convert from ADC units to uV + reading = reading * 4880; + // Adjust for voltage divider circuit + reading = (reading * VOLTDIVATOR) / 10; + // Convert to mV + reading = reading / 1000; + + return word(reading); } diff --git a/Liam/Battery.h b/Liam/Battery.h index 4dc6129..40f9585 100644 --- a/Liam/Battery.h +++ b/Liam/Battery.h @@ -13,27 +13,25 @@ #ifndef _BATTERY_H_ #define _BATTERY_H_ -#define LEAD_ACID 0 // this device only has one address -#define NIMH 1 -#define LIION 2 +// Voltages should be specified in mV (12.56 V = 12560 mV) +#define LIIONFULL 12560 +#define LIIONEMPTY 10400 +#define NIMHFULL 14500 +#define NIMHEMPTY 11500 +#define LEADACIDFULL 13300 +#define LEADACIDEMPTY 12000 -//All voltage value need to be x100 to avoid decimals (12.54V = 1254) -#define LIIONFULL 1256 -#define LIIONEMPTY 1040 -#define NIMHFULL 1450 -#define NIMHEMPTY 1150 -#define LEADACIDFULL 1330 -#define LEADACIDEMPTY 1200 +// Running average sample size +#define FILTER 5 -#define FILTER 5 //how hard to filter voltage readings -#define VOLTDIVATOR 42 //Voltage divider faktor x10 to avoid decimal ( 4 = 40) +// Voltage divider factor x10 to avoid decimal ( 4 = 40) +#define VOLTDIVATOR 42 class BATTERY { public: - BATTERY(int type, int socpin, int dockpin); + BATTERY(int type, int sensepin, int dockpin); - // int getBatteryType(); void setFullyChargedLevel(int level); @@ -46,18 +44,18 @@ class BATTERY { bool isBeingCharged(); bool isFullyCharged(); - void updateSOC(); - void resetSOC(); - int getSOC(); + int getVoltage(); + void resetVoltage(); + void updateVoltage(); word readBatteryAndCalcValue(); private: int batType; - int batSocpin; - int batDockpin; + int batSensePin; + int batDockPin; int fullyChargedLevel; int depletedLevel; - word averageSOC; + word averageVoltage; }; #endif /* _BATTERY_H_ */ diff --git a/Liam/Definition.cpp b/Liam/Definition.cpp index df9fd3a..42b16e0 100644 --- a/Liam/Definition.cpp +++ b/Liam/Definition.cpp @@ -21,7 +21,7 @@ void DEFINITION::definePinsInputOutput() { pinMode(WHEEL_MOTOR_A_CURRENT_PIN, INPUT); pinMode(WHEEL_MOTOR_B_CURRENT_PIN, INPUT); - pinMode(SOC_PIN, INPUT); + pinMode(BAT_PIN, INPUT); pinMode(CUTTER_CURRENT_PIN, INPUT); /* Some pins are better leave undefined as default diff --git a/Liam/Definition.h b/Liam/Definition.h index b07a3d2..e1ca74e 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -21,7 +21,7 @@ const int NUMBER_OF_SENSORS = 2; // Number of BWF sensors can be 1-4 depending // Pin setup following Morgan 1.5 shield and up #define WHEEL_MOTOR_A_CURRENT_PIN 0 #define WHEEL_MOTOR_B_CURRENT_PIN 1 -#define SOC_PIN 2 +#define BAT_PIN 2 #define CUTTER_CURRENT_PIN 3 #define I2C_SDA_PIN 4 #define I2C_SDL_PIN 5 @@ -47,9 +47,9 @@ const int NUMBER_OF_SENSORS = 2; // Number of BWF sensors can be 1-4 depending #define NIDEC 2 // Battery -#define LEADACID 0 -#define NIMH 1 -#define LIION 2 +#define LEAD_ACID 0 +#define NIMH 1 +#define LIION 2 // Wheel motor #define WHEELMOTOR_OVERLOAD 130 diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 0e2dcb6..d39bbbb 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -83,7 +83,7 @@ WHEELMOTOR leftMotor(WHEEL_MOTOR_B_PWM_PIN, WHEEL_MOTOR_B_DIRECTION_PIN, WHEEL_M // Battery // Battery types available are LIION, LEAD_ACID, NIMH -BATTERY Battery(LIION, SOC_PIN, DOCK_PIN); +BATTERY Battery(LIION, BAT_PIN, DOCK_PIN); // BWF Sensors BWFSENSOR Sensor(BWF_SELECT_B_PIN, BWF_SELECT_A_PIN); @@ -133,7 +133,7 @@ void setup() Display.initialize(); // Start up the display CutterMotor.initialize(); - Battery.resetSOC(); // Set the SOC to current value + Battery.resetVoltage(); // Reset the battery voltage reading Compass.initialize(); #if defined __RTC_CLOCK__ @@ -213,7 +213,7 @@ void loop() //------------------------- MOWING --------------------------- case MOWING: - Battery.updateSOC(); + Battery.updateVoltage(); Display.update(); Sensor.select(0); @@ -226,7 +226,7 @@ void loop() // Check left sensor (0) and turn right if needed if (mower_is_outside) { Serial.println("Left outside"); - Serial.println(Battery.getSOC()); + Serial.println(Battery.getVoltage()); Mower.stop(); #ifdef GO_BACKWARD_UNTIL_INSIDE err=Mower.GoBackwardUntilInside (&Sensor); @@ -273,7 +273,7 @@ void loop() // Check right sensor (1) and turn left if needed if (mower_is_outside) { Serial.println("Right Outside"); - Serial.println(Battery.getSOC()); + Serial.println(Battery.getVoltage()); Mower.stop(); #ifdef GO_BACKWARD_UNTIL_INSIDE @@ -397,7 +397,7 @@ void loop() state = MOWING; // Reset the running average - Battery.resetSOC(); + Battery.resetVoltage(); break; @@ -468,8 +468,8 @@ void loop() Mower.stop(); } - Serial.print("SOC:"); - Serial.println(Battery.getSOC()); + Serial.print("BAT:"); + Serial.println(Battery.getVoltage()); break; diff --git a/Liam/MyDisplay.cpp b/Liam/MyDisplay.cpp index c205625..7fcd166 100644 --- a/Liam/MyDisplay.cpp +++ b/Liam/MyDisplay.cpp @@ -84,9 +84,9 @@ void MYDISPLAY::update() { //Rad 3: Battery data setCursor(0,2); - print("Batt Soc:"); - setCursor(10,2); - print(Battery->getSOC()); + print("Battery:"); + setCursor(9,2); + print(Battery->getVoltage()); //Rad 4: State and Error data setCursor(0,3); diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index 32a1e21..8d21435 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -117,9 +117,9 @@ void SETUPDEBUG::startListeningOnSerial() { Serial.print(leftMotor->getLoad()); Serial.print(" RMot: "); Serial.print(rightMotor->getLoad()); - Serial.print(" SOC: "); - battery->resetSOC(); - Serial.print(battery->getSOC()); + Serial.print(" BAT: "); + battery->resetVoltage(); + Serial.print(battery->getVoltage()); Serial.print(" Dock: "); Serial.print(battery->isBeingCharged()); @@ -187,7 +187,7 @@ void SETUPDEBUG::startListeningOnSerial() { // // // } - // Serial.println("P = print SOC & debug values"); + // Serial.println("P = print battery & debug values"); // Serial.println("E = Cutter motor calibrate"); inChar = 0; @@ -204,7 +204,7 @@ void SETUPDEBUG::printHelp() { _Serial->println("G = test Gyro/Compass/Accelerometer"); _Serial->println("D = turn LED on/off"); _Serial->println("T = make a 10 second test run"); - _Serial->println("P = print SOC & debug values"); + _Serial->println("P = print battery & debug values"); _Serial->println("E = Cutter motor calibrate"); } From 796e5d8f3d9f0c5be01fe899d63ba6f08cdc77c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Mon, 14 May 2018 15:46:28 +0200 Subject: [PATCH 02/28] Cleaning up BWF library - Removed unused variables - Added comment explaining the principle of operation - Renamed some variables - Reordered some functions to match between definition and implementation --- Liam/BWFSensor.cpp | 90 +++++++++++++++++++++++++++++++--------------- Liam/BWFSensor.h | 36 +++++++++---------- 2 files changed, 78 insertions(+), 48 deletions(-) diff --git a/Liam/BWFSensor.cpp b/Liam/BWFSensor.cpp index 7993ffa..84facd0 100644 --- a/Liam/BWFSensor.cpp +++ b/Liam/BWFSensor.cpp @@ -7,32 +7,62 @@ Licensed under GPLv3 ====================== */ +/* + The BWF sensor works by measuring the time between rising edges + + Example: + Signal in BWF + + I + ^ + | _ _ + | | | | | + | | | | | + |...__| | _____| | ___... + | | | | | + | | | | | + | |_| |_| + | + +----------------------> t + 1 1 5 1 1 5... + + Outside of the fence, the sensor coil and amplifier circuit will sense the + rising edges in the signal. Inside the fence, the signal is inverted, and + the circuit will sense the falling edges of the signal instead. + + In this example, the time between rising edges is 2 units, followed by 5, + 2, 5, and so on. The time between falling edges is 7 units. + + When a rising edge is detected on the currently selected sensor, the function + readSensor() is run. By keeping track of the last time it was run, it can + calculate the time between pulses and check if it matches what was expected + for being inside or outside of the fence. +*/ #include "BWFSensor.h" int BWFSENSOR::outside_code[] = {OUTSIDE_BWF}; int BWFSENSOR::inside_code[] = {INSIDE_BWF}; -/** Specific constructor. -*/ BWFSENSOR::BWFSENSOR(int selA, int selB) { selpin_A = selA; selpin_B = selB; } -// select this sensor to be active +// Select active sensor void BWFSENSOR::select(int sensornumber) { digitalWrite(selpin_A, (sensornumber & 1) > 0 ? HIGH : LOW); digitalWrite(selpin_B, (sensornumber & 2) > 0 ? HIGH : LOW); clearSignal(); - delay(200); // Wait a little to collect signal + // Wait to allow signal to be read + delay(200); } void BWFSENSOR::clearSignal() { - for (int i=0; iarr_length) arr_count=0; + // Convert to pulse units (rounding up) + int pulse_length = (time_since_pulse+(pulse_unit_length/2)) / pulse_unit_length; // Check if the latest pulse fits the code for inside - if (abs(pulse_unit-inside_code[pulse_count_inside]) < 2) { + if (abs(pulse_length-inside_code[pulse_count_inside]) < 2) { pulse_count_inside++; - // If the whole code sequence has been OK, then set signal status to 1 + + // Check if the entire pulse train has been batched if (pulse_count_inside >= sizeof(inside_code)/sizeof(inside_code[0])) { signal_status = INSIDE; - signal_status_checked = millis(); + last_match = millis(); pulse_count_inside=0; } - } - else + } else { pulse_count_inside=0; + } // Check if the latest pulse fits the code for outside - if (abs(pulse_unit-outside_code[pulse_count_outside]) < 2) { + if (abs(pulse_length-outside_code[pulse_count_outside]) < 2) { pulse_count_outside++; if (pulse_count_outside >= sizeof(outside_code)/sizeof(outside_code[0])) { signal_status = OUTSIDE; - signal_status_checked = millis(); + last_match = millis(); pulse_count_outside=0; } - } - else + } else { pulse_count_outside=0; + } + + // Store the received code for debug output + arr[arr_count++] = pulse_length; + if (arr_count>arr_length) arr_count=0; } void BWFSENSOR::printSignal() { - - for (int i=0; i Date: Tue, 15 May 2018 10:53:07 +0200 Subject: [PATCH 03/28] Cleaning up Clock library Added a constructor to make the interface more similar to all other libraries. Added a setTime function for use with future configuration interfaces. --- Liam/Clock.cpp | 18 +++++++++--------- Liam/Clock.h | 3 ++- Liam/Liam.ino | 12 +++--------- 3 files changed, 14 insertions(+), 19 deletions(-) diff --git a/Liam/Clock.cpp b/Liam/Clock.cpp index 887bdcd..bfc51bb 100644 --- a/Liam/Clock.cpp +++ b/Liam/Clock.cpp @@ -11,34 +11,35 @@ #include "Clock.h" #include "Definition.h" -void CLOCK::initialize() { +CLOCK::CLOCK(uint8_t outHour, uint8_t outMinute, uint8_t inHour, uint8_t inMinute) { + outTimeHour = outHour; + outTimeMinutes = outMinute; + inTimeHour = outHour; + inTimeMinutes = outMinute; Wire.begin(); RTC.begin(); if (! RTC.isrunning()) { Serial.println("RTC is NOT running!"); - // following line sets the RTC to the date & time this sketch was compiled - RTC.adjust(DateTime(F(__DATE__), F(__TIME__))); } } -void CLOCK::setGoOutTime(uint8_t Hour, uint8_t Minutes) { +void CLOCK::setTime(uint16_t year, uint8_t month, uint8_t day, uint8_t hours, uint8_t minutes, uint8_t seconds) { + RTC.adjust(DateTime(year, month, day, hours, minutes, seconds)); +} +void CLOCK::setGoOutTime(uint8_t Hour, uint8_t Minutes) { outTimeHour = Hour; outTimeMinutes = Minutes; - } void CLOCK::setGoHomeTime(uint8_t Hour, uint8_t Minutes) { - inTimeHour = Hour; inTimeMinutes = Minutes; - } bool CLOCK::timeToCut() { - if ((int)RTC.now().hour() * 60 + (int)RTC.now().minute() > (int)outTimeHour * 60 + (int)outTimeMinutes && (int)RTC.now().hour() * 60 + (int)RTC.now().minute() < (int)inTimeHour * 60 + (int)inTimeMinutes) return true; @@ -46,7 +47,6 @@ bool CLOCK::timeToCut() { return false; } - void CLOCK::printTime() { DateTime now = RTC.now(); diff --git a/Liam/Clock.h b/Liam/Clock.h index d5c4afb..f1c9962 100644 --- a/Liam/Clock.h +++ b/Liam/Clock.h @@ -18,8 +18,9 @@ class CLOCK { public: - void initialize(); + CLOCK(uint8_t outHour, uint8_t outMinute, uint8_t inHour, uint8_t inMinute); + void setTime(uint16_t year, uint8_t month, uint8_t day, uint8_t hours, uint8_t minutes, uint8_t seconds); void setGoOutTime(uint8_t Hour, uint8_t Minutes); void setGoHomeTime(uint8_t Hour, uint8_t Minutes); diff --git a/Liam/Liam.ino b/Liam/Liam.ino index d39bbbb..91f49c8 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -107,9 +107,9 @@ myLCD Display(&Battery, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass MYDISPLAY Display(&Battery, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &state); #endif -// RTC klocka +// RTC clock #if defined __RTC_CLOCK__ -CLOCK myClock; +CLOCK Clock(GO_OUT_TIME, GO_HOME_TIME); #endif // Error handler @@ -136,12 +136,6 @@ void setup() Battery.resetVoltage(); // Reset the battery voltage reading Compass.initialize(); -#if defined __RTC_CLOCK__ - myClock.initialize(); - myClock.setGoOutTime(GO_OUT_TIME); - myClock.setGoHomeTime(GO_HOME_TIME); -#endif - attachInterrupt(0, updateBWF, RISING); // Run the updateBWF function every time there is a pulse on digital pin2 Sensor.select(0); @@ -443,7 +437,7 @@ void loop() // Just remain in this state until battery is full #if defined __RTC_CLOCK__ - if (Battery.isFullyCharged() && myClock.timeToCut()) + if (Battery.isFullyCharged() && Clock.timeToCut()) state = LAUNCHING; #else if (Battery.isFullyCharged()) From dc8109bc85afd7f6c164aa4820cc50a7e749f550 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Tue, 15 May 2018 14:25:48 +0200 Subject: [PATCH 04/28] Cleaning up CutterMotor library - Replaced magic numbers with constants from Definition.h - Stop motor after initialization. Important e.g. for NIDEC, which otherwise spins at full speed by default. - Removed brake functions. They were unimplemented and unused. - Removed some unused variables. --- Liam/CutterMotor.cpp | 50 +++++++++++++++----------------------------- Liam/CutterMotor.h | 7 ------- 2 files changed, 17 insertions(+), 40 deletions(-) diff --git a/Liam/CutterMotor.cpp b/Liam/CutterMotor.cpp index ec3a7e7..bf64844 100644 --- a/Liam/CutterMotor.cpp +++ b/Liam/CutterMotor.cpp @@ -7,38 +7,36 @@ Licensed under GPLv3 ====================== */ - /* - The motor can be either brushed or brushless + The cutter motor can be of type BRUSHLESS, BRUSHED or + NIDEC - Motor speed is defined as percentage of full speed. A - speed of 100 means full speed and 0 is stop. Speed can - also be negative if reverse direction is requested. + BRUSHLESS is assumed to be connected via an ESC. + BRUSHED and NIDEC are controlled via PWM, but the signal + to NIDEC is inverted. - Current draw of the motor can be read using the getLoad() - method + Speed can be set between 0 and 100. */ #include "CutterMotor.h" +#include "Definition.h" -/** Specific constructor. -*/ CUTTERMOTOR::CUTTERMOTOR(int type_, int pwmpin_, int loadpin_) { type = type_; pwmpin = pwmpin_; loadpin = loadpin_; - } void CUTTERMOTOR::initialize() { // Initialize if brushless with ESC - if (type == 0) { + if (type == BRUSHLESS) { cutter.attach(pwmpin); cutter.writeMicroseconds(2000); delay(400); cutter.writeMicroseconds(1000); delay(2000); } + setSpeed(0); } @@ -47,16 +45,15 @@ void CUTTERMOTOR::setSpeed(int setspeed) { if (speed > 100) speed = 100; if (speed < 0) speed = 0; - if (type == 0) // brushless - pwm = abs(10*speed)+1000; - else if (type == 1) // brushed - pwm = abs(2.55*speed); - else if (type == 2) // Nidec - pwm = 255 - abs(2.55*speed); + int pwm; + if (type == BRUSHLESS) + pwm = 1000 + 1000 * speed/100; + else if (type == BRUSHED) + pwm = 255 * speed/100; + else if (type == NIDEC) + pwm = 255 - 255 * speed/100; - braking = false; - - if (type == 0) + if (type == BRUSHLESS) cutter.writeMicroseconds(pwm); else analogWrite(pwmpin, pwm); @@ -67,23 +64,10 @@ int CUTTERMOTOR::getSpeed() { return speed; } - int CUTTERMOTOR::getLoad() { return analogRead(loadpin); } - - -void CUTTERMOTOR::brake() { - setSpeed(0); - braking = true; -} - - -bool CUTTERMOTOR::isBraking() { - return braking; -} - bool CUTTERMOTOR::isOverloaded() { return (getLoad() > overload_level); } diff --git a/Liam/CutterMotor.h b/Liam/CutterMotor.h index 8887ecb..291044a 100644 --- a/Liam/CutterMotor.h +++ b/Liam/CutterMotor.h @@ -18,16 +18,12 @@ class CUTTERMOTOR { public: CUTTERMOTOR(int type_, int pwmpin_, int loadpin_); - // void initialize(); void setSpeed(int setspeed); int getSpeed(); int getLoad(); - void brake(); - bool isBraking(); - bool isBrushless(); bool isOverloaded(); void setOverloadLevel(int level); @@ -36,10 +32,7 @@ class CUTTERMOTOR { int pwmpin; int pwm; int type; - int brakepin; - bool braking; int loadpin; - int load; int speed; int overload_level; Servo cutter; From 3a33894ce6cee758a43f80bd712f7fe5b9b52dd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Tue, 15 May 2018 19:49:25 +0200 Subject: [PATCH 05/28] Cleaning up WheelMotor library - Simplified the speed ramping a bit. - Replaced magic number with constant in header file. - Removed some unused variables. --- Liam/Wheelmotor.cpp | 40 ++++++++++++---------------------------- Liam/Wheelmotor.h | 5 +++-- 2 files changed, 15 insertions(+), 30 deletions(-) diff --git a/Liam/Wheelmotor.cpp b/Liam/Wheelmotor.cpp index ce1116b..da76e0d 100644 --- a/Liam/Wheelmotor.cpp +++ b/Liam/Wheelmotor.cpp @@ -8,26 +8,10 @@ ====================== */ -/* - Motor speed is defined as percentage of full speed. A - speed of 100 means full speed and 0 is stop. Speed can - also be negative if reverse direction is requested. - - Current draw of the motor can be read using the getLoad() - method -*/ - #include "Wheelmotor.h" -/** Specific constructor. -*/ WHEELMOTOR::WHEELMOTOR(int pwmpin_, int dirpin_, int loadpin_, int smoothness) { - // Set up PWM frequency for pin 3 and 11 to 3921 Hz for smooth running - // At default pwm, the frequency of the motors will disturb the BWFsensors - // This is very evident if using chinese wheelmotors - // TCCR2B = TCCR2B & 0b11111000 | 0x02; - pwmpin = pwmpin_; dirpin = dirpin_; loadpin = loadpin_; @@ -36,22 +20,22 @@ WHEELMOTOR::WHEELMOTOR(int pwmpin_, int dirpin_, int loadpin_, int smoothness) { void WHEELMOTOR::setSpeed(int setspeed) { - int diff = 1-2*((setspeed-speed) < 0); - int stepnr = abs(setspeed-speed); - if (setspeed > 100) setspeed = 100; if (setspeed < -100) setspeed = -100; - dir = (setspeed > 0); + // Increase or decrease speed? + int diff = (setspeed < speed)? -1 : 1; - for (int i=0; i 0)); - delayMicroseconds(smoothness_delay); // Smooth ramping of motors - } - speed = setspeed; + delayMicroseconds(smoothness_delay); + } } @@ -61,14 +45,14 @@ int WHEELMOTOR::getSpeed() { int WHEELMOTOR::getLoad() { - load = 0; + int load = 0; - for (int i=0; i<10; i++) { + for (int i = 0; i < MOTOR_LOAD_READINGS; i++) { load += analogRead(loadpin); delay(1); } - return load/10; + return load/MOTOR_LOAD_READINGS; } diff --git a/Liam/Wheelmotor.h b/Liam/Wheelmotor.h index 410f813..5e44b05 100644 --- a/Liam/Wheelmotor.h +++ b/Liam/Wheelmotor.h @@ -13,6 +13,9 @@ #include +// Number of readings to average when measuring motor load +#define MOTOR_LOAD_READINGS 10 + class WHEELMOTOR { public: WHEELMOTOR(int pwmpin_, int dirpin_, int loadpin_, int smoothness); @@ -30,9 +33,7 @@ class WHEELMOTOR { private: int pwmpin; int dirpin; - bool dir; int loadpin; - int load; int speed; int overload_level; int smoothness_delay; From ced5ff91cfe7822e8cfa9094c0bfe63dbd003f7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Fri, 18 May 2018 12:35:08 +0200 Subject: [PATCH 06/28] Cleanup of myDisplay and myLCD libraries - Removed current_row and current_col from MYDISPLAY. It makes more sense to keep them in subclasses that need them. - Generalized update() function. - Moved printing of version information to main program instead of display library. - Added F() around all strings to save on RAM. - Removed duplicate code from myLCD library. - Added comment with brief explanation on how to add display types --- Liam/Definition.h | 6 +++ Liam/Liam.ino | 8 +++ Liam/MyDisplay.cpp | 121 +++++++++++++++++++-------------------------- Liam/MyDisplay.h | 17 +++---- Liam/myLcd.cpp | 78 ++++++++++++++++------------- Liam/myLcd.h | 17 +++++-- 6 files changed, 128 insertions(+), 119 deletions(-) diff --git a/Liam/Definition.h b/Liam/Definition.h index e1ca74e..01d4b48 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -89,6 +89,12 @@ const int CHARGING = 3; #define MINOR_VERSION_1 2 #define MINOR_VERSION_2 2 +// A bit of macro magic to make a string out of the version number +// The preprocessor works in mysterious ways... +#define STR_(x) #x +#define STR(x) STR_(x) +#define VERSION_STRING STR(MAJOR_VERSION) "." STR(MINOR_VERSION_1) "." STR(MINOR_VERSION_2) + // If you have a bumper connected to pin8, uncomment the line below. Remember to cut the brake connection on your motor shield //#define __Bumper__ diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 91f49c8..57dda86 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -139,6 +139,14 @@ void setup() attachInterrupt(0, updateBWF, RISING); // Run the updateBWF function every time there is a pulse on digital pin2 Sensor.select(0); + // Print version information for five seconds before starting + Display.clear(); + Display.print(F("--- LIAM ---\n")); + Display.print(F(VERSION_STRING "\n")); + Display.print(__DATE__ " " __TIME__ "\n"); + delay(5000); + Display.clear(); + if (Battery.isBeingCharged()) { // If Liam is in docking station then state = CHARGING; // continue charging Mower.stopCutter(); diff --git a/Liam/MyDisplay.cpp b/Liam/MyDisplay.cpp index 7fcd166..fca3387 100644 --- a/Liam/MyDisplay.cpp +++ b/Liam/MyDisplay.cpp @@ -7,11 +7,19 @@ Licensed under GPLv3 ====================== */ - /* - It will by default write to serial port, but can be - replaced by subclasses for a built in LCD or OLED for - example + Base class for displays. + Default behavior is to print to serial port. + + Subclasses should implement the following functions: + - size_t write(uint8_t) + - void setCursor(int col, int row) + - void clear() + - void blink(); + + If necesary, also override + - boolean initialize() + but make sure to also call MYDISPLAY::initialize() */ #include "MyDisplay.h" @@ -27,116 +35,91 @@ MYDISPLAY::MYDISPLAY(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERM moverstate = state; } -// Do not override -boolean MYDISPLAY::initialize() { - char buffer [9]; //Format 09.00.00 - - current_row = 0; - current_col = 0; - +boolean MYDISPLAY::initialize() +{ for (int i=0; i<3; i++) blink(); - setCursor(0, 0); - print("SW version:"); - setCursor(0, 1); - sprintf (buffer, "%d.%d.%d", MAJOR_VERSION, MINOR_VERSION_1, MINOR_VERSION_2); - print(buffer); - setCursor(0, 2); - print(__DATE__); - setCursor(0, 3); - print(__TIME__); - delay(3000); clear(); + + return true; } // Do NOT override. Implement basic commands here -void MYDISPLAY::update() { - int sens = 0; - - // Rad 1: Sensors -#if __MS9150__ || __MS5883L__ || __ADXL345__ || __MMA7455__ +void MYDISPLAY::update() +{ + // Row 1: Sensor status setCursor(0,0); - print("Comp:"); - setCursor(7,0); +#if __MS9150__ || __MS5883L__ || __ADXL345__ || __MMA7455__ + print(F("Comp: ")); print(compass->getHeading()); #else - setCursor(0,0); - print("Sens:"); - setCursor(7,0); - print("Disabled"); + print(F("Sens: ")); + print(F("Disabled")); #endif - setCursor(10,0); - print("InOut:"); - setCursor(17,0); + print(F("InOut: ")); print(sensor->isInside()); - //Rad 2: Motor loading - setCursor(0,1); - print("LMoto:"); - setCursor(7,1); + print("\n"); + // Row 2: Motor load + print(F("LMoto: ")); print(leftMotor->getLoad()); - setCursor(10,1); - print("RMoto:"); - setCursor(17,1); + print(F(" RMoto: ")); print(rightMotor->getLoad()); - //Rad 3: Battery data - setCursor(0,2); - print("Battery:"); - setCursor(9,2); + print("\n"); + // Row 3: Battery + print(F("Battery: ")); print(Battery->getVoltage()); - //Rad 4: State and Error data - setCursor(0,3); - print("State:"); - setCursor(7,3); + print("\n"); + // Row 4: State and Error data + print(F("State:")); - switch (*moverstate) { + switch (*moverstate) + { case MOWING: - print("MOWING"); + print(F("MOWING")); break; case LAUNCHING: - print("LAUNCHING"); + print(F("LAUNCHING")); break; case DOCKING: - print("DOCKING"); + print(F("DOCKING")); break; case CHARGING: - print("CHARGING"); + print(F("CHARGING")); break; } } -// This is the basic implementation of the print and println command + +// DEVICE SPECIFIC FUNCTIONS + // Override this for each type of display -size_t MYDISPLAY::write(uint8_t s) { +size_t MYDISPLAY::write(uint8_t s) +{ // By default just write to serial port - Serial.write(s); + return Serial.write(s); } // Override this for each type of display -void MYDISPLAY::setCursor(int col, int row) { +void MYDISPLAY::setCursor(int col, int row) +{ // For a serial port, do nothing - current_col = col; - current_row = row; } // Override this for each type of display -void MYDISPLAY::clear() { +void MYDISPLAY::clear() +{ // For a serial port, do very little println(); - setCursor(0,0); } // Override this for each type of display -void MYDISPLAY::blink() { +void MYDISPLAY::blink() +{ // For a serial port, do very little - println(""); - delay(1000); - //println("**********************"); println("*"); - delay(1000); - setCursor(0,0); } diff --git a/Liam/MyDisplay.h b/Liam/MyDisplay.h index f0a0974..507a8bc 100644 --- a/Liam/MyDisplay.h +++ b/Liam/MyDisplay.h @@ -9,9 +9,8 @@ */ /* - It will by default write to serial port, but can be - replaced by subclasses for a built in LCD or OLED for - example + Base class for displays. + Default behavior is to print to serial port. This code extends the Print class to get the print capabilities. Basically the print class method calls each @@ -29,19 +28,19 @@ #include "MotionSensor.h" #include "Battery.h" -class MYDISPLAY : public Print { +class MYDISPLAY : public Print +{ public: MYDISPLAY(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, int* state); virtual boolean initialize(); - virtual void update(); - virtual void setCursor(int col, int row); + void update(); + virtual size_t write(uint8_t); + virtual void setCursor(int col, int row); virtual void clear(); virtual void blink(); protected: - int current_row; - int current_col; BATTERY* Battery; WHEELMOTOR* leftMotor; WHEELMOTOR* rightMotor; @@ -49,8 +48,6 @@ class MYDISPLAY : public Print { BWFSENSOR* sensor; MOTIONSENSOR* compass; int* moverstate; - - }; #endif /* _MYDISPLAY_H_ */ diff --git a/Liam/myLcd.cpp b/Liam/myLcd.cpp index 2ccf626..627dfa1 100644 --- a/Liam/myLcd.cpp +++ b/Liam/myLcd.cpp @@ -9,58 +9,66 @@ */ #include "myLcd.h" +#include +myLCD::myLCD(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, int* state) : + MYDISPLAY(batt, left, right, cut, bwf, comp, state), + // You may need to modify this line to work with your LCD controller + lcd(LCD_I2C_ADDRESS, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE) +{ +} -myLCD::myLCD(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, int* state): MYDISPLAY(batt, left, right, cut, bwf, comp, state), lcd(LCD_I2C_ADDRESS, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE) +boolean myLCD::initialize() { - Battery = batt; - leftMotor = left; - rightMotor = right; - cutter = cut; - sensor = bwf; - compass = comp; - moverstate = state; + current_row = current_col = 0; + lcd.begin(max_cols, max_rows); + + // Hide cursor and turn on backlight + lcd.noCursor(); + lcd.backlight(); + + return MYDISPLAY::initialize(); } -boolean myLCD::initialize() +size_t myLCD::write(uint8_t s) { - char buffer [9]; //Format 09.00.00 + current_col++; - lcd.begin(20,4); // initialize the lcd for 20 chars 4 lines, turn on backlight - lcd.noBacklight(); - // ------- Quick 3 blinks of backlight ------------- - for(int i = 0; i < 3; i++) + if(s == '\n' || current_col >= max_cols) { - lcd.backlight(); - delay(100); - lcd.noBacklight(); - delay(100); + current_row++; + current_col = 0; + if(current_row >= max_rows) + current_row = 0; + lcd.setCursor(current_col, current_row); } - lcd.backlight(); // finish with backlight on - lcd.setCursor(0, 0); - lcd.print("SW version:"); - lcd.setCursor(0, 1); - sprintf (buffer, "%d.%d.%d", MAJOR_VERSION, MINOR_VERSION_1, MINOR_VERSION_2); - lcd.print(buffer); - lcd.setCursor(0, 2); - lcd.print(__DATE__); - lcd.setCursor(0, 3); - lcd.print(__TIME__); - delay(3000); - lcd.clear(); + lcd.write(s); + // Uncomment to write to serial port also + /* MYDISPLAY::write(s); */ } -void myLCD::setCursor(int col, int row) { +void myLCD::setCursor(int col, int row) +{ + current_row = row; + current_col = col; lcd.setCursor(col, row); } -size_t myLCD::write(uint8_t s) { - lcd.write(s); +void myLCD::clear() +{ + lcd.clear(); + setCursor(0,0); } -void myLCD::clear() { - lcd.clear(); +void myLCD::blink() +{ + // Flash backlight + lcd.backlight(); + delay(100); + lcd.noBacklight(); + delay(100); + lcd.backlight(); } diff --git a/Liam/myLcd.h b/Liam/myLcd.h index 1f25011..6f31416 100644 --- a/Liam/myLcd.h +++ b/Liam/myLcd.h @@ -22,17 +22,24 @@ #ifndef _MYLCD_H_ #define _MYLCD_H_ -class myLCD : public MYDISPLAY { +class myLCD : public MYDISPLAY +{ public: myLCD(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, int* state); - virtual boolean initialize(); - virtual void setCursor(int col, int row); - virtual size_t write(uint8_t); - virtual void clear(); + boolean initialize(); + + size_t write(uint8_t); + void setCursor(int col, int row); + void clear(); + void blink(); private: LiquidCrystal_I2C lcd; + int current_row; + int current_col; + const int max_rows = 4; + const int max_cols = 20; }; #endif /* _MYLCD_H_ */ From 5f0bddd6ab9e2ab98db34c5161b89a7e917c8148 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Fri, 18 May 2018 14:32:27 +0200 Subject: [PATCH 07/28] Cleanup of Error library - Abbreviated error messages - Removed unused errors - Replaced error numbers with constants --- Liam/Controller.cpp | 11 ++-- Liam/Error.cpp | 123 +++++++++++++------------------------------- Liam/Error.h | 12 +++-- Liam/Liam.ino | 12 ++--- 4 files changed, 57 insertions(+), 101 deletions(-) diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index effc53c..5ac358c 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -10,6 +10,7 @@ #include "Controller.h" #include "Definition.h" +#include "Error.h" /** Specific constructor. */ @@ -48,12 +49,12 @@ int CONTROLLER::turnToReleaseLeft(int angle) { } if (wheelsAreOverloaded()) - return 1; // Overloaded + return ERROR_OVERLOAD; // Overloaded turnLeft(10); } - return 2; // Timed Out + return ERROR_OUTSIDE; // Timed Out } int CONTROLLER::turnToReleaseRight(int angle) { @@ -69,12 +70,12 @@ int CONTROLLER::turnToReleaseRight(int angle) { } if (wheelsAreOverloaded()) - return 1; // Overloaded + return ERROR_OVERLOAD; // Overloaded turnRight(10); } - return 2; // Timed Out + return ERROR_OUTSIDE; // Timed Out } int CONTROLLER::turnRight(int angle) { @@ -147,7 +148,7 @@ int CONTROLLER::GoBackwardUntilInside (BWFSENSOR *Sensor) { delay(1000); counter--; if(counter<=0) - return 1; + return ERROR_OUTSIDE; } return 0; } diff --git a/Liam/Error.cpp b/Liam/Error.cpp index 77b7dea..0ce175d 100644 --- a/Liam/Error.cpp +++ b/Liam/Error.cpp @@ -10,101 +10,50 @@ #include "Error.h" -/** Specific constructor. -*/ ERROR::ERROR(MYDISPLAY* display_, int led_pin_, CONTROLLER* Mower_) { - mylcd = display_; + display = display_; led_pin = led_pin_; Mower = Mower_; } -void ERROR::flag(int error_number_) { - - Mower->stopCutter(); - Mower->stop(); - mylcd->clear(); - - mylcd->setCursor(5,0); - mylcd->print("Error!!"); - switch (error_number_) { - case 1: - mylcd->setCursor(0,1); - mylcd->print("Mower is"); - mylcd->setCursor(0,2); - mylcd->print("outside"); - mylcd->setCursor(0,3); - mylcd->print("Boundaries"); - break; - case 2: - mylcd->setCursor(0,1); - mylcd->print("No Signal"); - mylcd->setCursor(0,2); - mylcd->print("From BFW"); - mylcd->setCursor(0,3); - mylcd->print(""); - break; - case 3: - mylcd->setCursor(0,1); - mylcd->print("Overloaded right"); - mylcd->setCursor(0,2); - mylcd->print("motor while docking"); - break; - case 4: - mylcd->setCursor(0,1); - mylcd->print("Overloaded left"); - mylcd->setCursor(0,2); - mylcd->print("motor while docking"); - break; - case 5: - mylcd->setCursor(0,1); - mylcd->print("Failed to align"); - mylcd->setCursor(0,2); - mylcd->print("perpend. left to"); - mylcd->setCursor(0,3); - mylcd->print("wire while tracking"); - break; - case 6: - mylcd->setCursor(0,1); - mylcd->print("Failed to align"); - mylcd->setCursor(0,2); - mylcd->print("perpend. right to"); - mylcd->setCursor(0,3); - mylcd->print("wire while tracking"); - break; - case 7: - mylcd->setCursor(0,1); - mylcd->print("Left sensor"); - mylcd->setCursor(0,2); - mylcd->print("triggered while"); - mylcd->setCursor(0,3); - mylcd->print("WaitAndCheck"); - break; - case 8: - mylcd->setCursor(0,1); - mylcd->print("Right sensor"); - mylcd->setCursor(0,2); - mylcd->print("triggered while"); - mylcd->setCursor(0,3); - mylcd->print("WaitAndCheck"); - break; - case 9: - mylcd->setCursor(0,2); - mylcd->print("Mower tilted"); - break; +String ERROR::errorMessage(int error_number) +{ + switch(error_number) + { +// 16 characters should fit on almost any LCD +// 20 should fit on most +// F("1234567890123456xxxx") + case ERROR_OUTSIDE: + return F("OUTSIDE BWF"); + case ERROR_OVERLOAD: + return F("WHEEL OVERLOAD"); + case ERROR_TILT: + return F("TILTED"); + case ERROR_LIFT: + return F("FRONT LIFTED"); + default: + return F("UNKNOWN ERROR"); } - - // blink LED forever - while (true) - blink_led(error_number_); } +void ERROR::flag(int error_number) { -void ERROR::blink_led(int error_number_){ - for (int i=0; istopCutter(); + Mower->stop(); + display->clear(); + + display->print("ERROR!"); + display->print("\n"); + display->print(errorMessage(error_number)); + + while(true) + { + for(int i = 0; i < error_number; i++) { + digitalWrite(led_pin, HIGH); + delay(500); + digitalWrite(led_pin, LOW); + delay(500); + } + delay(2000); } - delay (2000); } diff --git a/Liam/Error.h b/Liam/Error.h index 3754773..3b52ebb 100644 --- a/Liam/Error.h +++ b/Liam/Error.h @@ -10,24 +10,30 @@ #include #include // For LCD -#include "myLcd.h" +#include "MyDisplay.h" #include "Controller.h" #ifndef _ERROR_H_ #define _ERROR_H_ +#define ERROR_OUTSIDE 1 +#define ERROR_OVERLOAD 2 +#define ERROR_TILT 3 +#define ERROR_LIFT 4 + + class ERROR { public: ERROR(MYDISPLAY* display_, int led_pin_, CONTROLLER* Mower_); + String errorMessage(int error_number); void flag(int error_number); private: - MYDISPLAY* mylcd; + MYDISPLAY* display; CONTROLLER* Mower; int led_pin; - void blink_led(int error_number); }; #endif /* _ERROR_H_ */ diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 57dda86..04e9010 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -178,7 +178,7 @@ void loop() Serial.print("Mower has flipped "); Mower.stopCutter(); Mower.stop(); - Error.flag(9); + Error.flag(ERROR_TILT); } #endif @@ -192,7 +192,7 @@ void loop() Mower.runBackward(FULLSPEED); delay(2000); if(Mower.isLifted()) - Error.flag(4); + Error.flag(ERROR_LIFT); Mower.turnRight(90); //Mover.startCutter(); Mower.runForward(FULLSPEED); @@ -261,7 +261,7 @@ void loop() delay(1000); Mower.stop(); if (Mower.allSensorsAreOutside()) - Error.flag(4); + Error.flag(ERROR_OUTSIDE); } } @@ -300,7 +300,7 @@ void loop() delay(1000); Mower.stop(); if (Mower.allSensorsAreOutside()) - Error.flag(4); + Error.flag(ERROR_OUTSIDE); } } @@ -346,7 +346,7 @@ void loop() Mower.runBackward(FULLSPEED); delay(2000); if(Mower.isLifted()) - Error.flag(4); + Error.flag(ERROR_LIFT); Mower.turnRight(90); Mower.startCutter(); Mower.runForward(FULLSPEED); @@ -371,7 +371,7 @@ void loop() Serial.print("Mower has flipped "); Mower.stopCutter(); Mower.stop(); - Error.flag(9); + Error.flag(ERROR_TILT); } #endif From 3b35cffa7663d9a4ebf5f5317b0b5b0edf8d2df0 Mon Sep 17 00:00:00 2001 From: Ola Palm Date: Tue, 29 May 2018 17:41:36 +0200 Subject: [PATCH 08/28] BWF uses timer instead of delay --- Liam/BWFSensor.cpp | 14 +++++++++++++- Liam/BWFSensor.h | 2 ++ Liam/Definition.h | 8 +++++++- Liam/Liam.ino | 4 ++++ 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/Liam/BWFSensor.cpp b/Liam/BWFSensor.cpp index 84facd0..9e5e4bd 100644 --- a/Liam/BWFSensor.cpp +++ b/Liam/BWFSensor.cpp @@ -55,8 +55,16 @@ void BWFSENSOR::select(int sensornumber) { digitalWrite(selpin_A, (sensornumber & 1) > 0 ? HIGH : LOW); digitalWrite(selpin_B, (sensornumber & 2) > 0 ? HIGH : LOW); clearSignal(); + long time = millis(); + while (!gotSignal()) + { + if (millis() - time >= BWF_COLLECT_SIGNAL_TIME) // max time of 200ms + { + signal_status = OUTSIDE; + } + } // Wait to allow signal to be read - delay(200); + // delay(200); } @@ -137,3 +145,7 @@ void BWFSENSOR::printSignal() { Serial.print(" "); } } +bool BWFSENSOR::gotSignal() +{ + return arr_count >= BWF_NUMBER_OF_PULSES ? true : false; +} \ No newline at end of file diff --git a/Liam/BWFSensor.h b/Liam/BWFSensor.h index 0b8d800..0c19965 100644 --- a/Liam/BWFSensor.h +++ b/Liam/BWFSensor.h @@ -22,6 +22,7 @@ #define TIMEOUT_DELAY 20000 #define NO_SIGNAL_DELAY 4000 + class BWFSENSOR { public: BWFSENSOR(int selA, int selB); @@ -33,6 +34,7 @@ class BWFSENSOR { bool isOutside(); bool isTimedOut(); bool hasNoSignal(); + bool gotSignal(); void readSensor(); diff --git a/Liam/Definition.h b/Liam/Definition.h index 01d4b48..779b1a6 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -69,10 +69,13 @@ const int CHARGING = 3; #define REVERSE_DELAY 2 #define TURNDELAY 20 //Reduce for smaller turning angle +#pragma region BWF + // BWF Detection method (true = always, false = only at wire) #define BWF_DETECTION_ALWAYS true #define TIMEOUT 6000 //Time without signal before error - +#define BWF_COLLECT_SIGNAL_TIME 200 // max time to spend looking for signal +#define BWF_NUMBER_OF_PULSES 3 // Trigger value for the mower to leave the BWF when going home // The higher value the more turns (in the same direction) the mower can make before leaving #define BALANCE_TRIGGER_LEVEL 10000 @@ -85,6 +88,9 @@ const int CHARGING = 3; #define INSIDE_BWF 85 #define OUTSIDE_BWF 5 +#pragma endregion BWF + + #define MAJOR_VERSION 5 #define MINOR_VERSION_1 2 #define MINOR_VERSION_2 2 diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 04e9010..1a7a9f2 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -163,6 +163,7 @@ void setup() // ***************** Main loop *********************************** void loop() { + long looptime= millis(); boolean in_contact; boolean mower_is_outside; int err=0; @@ -476,5 +477,8 @@ void loop() break; } + Serial.print("looptime == "); + Serial.print(millis() - looptime); + } From a8ff105ab2f70f618cc1f60b73c99134bdf68399 Mon Sep 17 00:00:00 2001 From: Ola Palm Date: Tue, 29 May 2018 18:31:14 +0200 Subject: [PATCH 09/28] Wheel are overloaded fix. --- Liam/Controller.cpp | 21 +++++++++++++++------ Liam/Liam.ino | 5 +++-- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index 5ac358c..47a6175 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -255,12 +255,21 @@ int CONTROLLER::compensateSpeedToCompassHeading() { rightMotor->setSpeed(default_dir_fwd*rms); } -boolean CONTROLLER::wheelsAreOverloaded() { - delay(200); // Settle current spikes - if (leftMotor->isOverloaded() | rightMotor->isOverloaded()) - return true; - else - return false; +boolean CONTROLLER::wheelsAreOverloaded() +{ + long time = millis(); + int l_load = 0; + int r_load = 0; + while (millis() - time <= 200) // might be better to set this value in Definition.h + { + l_load = leftMotor->getLoad(); + r_load = rightMotor->getLoad(); + if (l_load < WHEELMOTOR_OVERLOAD || r_load < WHEELMOTOR_OVERLOAD) + { + return false; + } + } + return true; } boolean CONTROLLER::hasBumped() { diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 1a7a9f2..8cafa1e 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -477,8 +477,9 @@ void loop() break; } - Serial.print("looptime == "); - Serial.print(millis() - looptime); + Serial.print("\n\nlooptime : "); + Serial.println(millis() - looptime); } + From dc4ca675069420a0b1f45a500fc6dd116277a6c5 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Sun, 27 May 2018 23:45:26 +0200 Subject: [PATCH 10/28] Ignore Visual Studio generated files --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index 94f1119..2d45eab 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,7 @@ .DS_Store .vscode +*/.vs/ +*/__vm/ +*/Debug/ +*.sln +*.vcxproj* From b02bee75aef621f8dbd4ff4ed1da51cc5e172917 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Tue, 29 May 2018 22:59:12 +0200 Subject: [PATCH 11/28] Added isOUtOfBounds. Faster loop. Random turning. Unified collision handling --- .gitignore | 1 + Liam/BWFSensor.cpp | 19 ++++- Liam/BWFSensor.h | 1 + Liam/Controller.cpp | 108 +++++++++++++++++++------ Liam/Controller.h | 2 + Liam/Liam.ino | 190 ++++++++++++++++++++++++++++---------------- Liam/Wheelmotor.cpp | 46 ++++++++++- Liam/Wheelmotor.h | 7 ++ 8 files changed, 274 insertions(+), 100 deletions(-) diff --git a/.gitignore b/.gitignore index 2d45eab..97837fc 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ */Debug/ *.sln *.vcxproj* +*.orig diff --git a/Liam/BWFSensor.cpp b/Liam/BWFSensor.cpp index 84facd0..7f8caa2 100644 --- a/Liam/BWFSensor.cpp +++ b/Liam/BWFSensor.cpp @@ -41,9 +41,11 @@ #include "BWFSensor.h" -int BWFSENSOR::outside_code[] = {OUTSIDE_BWF}; +int BWFSENSOR::outside_code[] = {OUTSIDE_BWF, INSIDE_BWF-OUTSIDE_BWF}; int BWFSENSOR::inside_code[] = {INSIDE_BWF}; +int currentSensor = 0; + BWFSENSOR::BWFSENSOR(int selA, int selB) { selpin_A = selA; selpin_B = selB; @@ -52,10 +54,16 @@ BWFSENSOR::BWFSENSOR(int selA, int selB) { // Select active sensor void BWFSENSOR::select(int sensornumber) { + if (currentSensor == sensornumber) { + + return; + } + currentSensor = sensornumber; + digitalWrite(selpin_A, (sensornumber & 1) > 0 ? HIGH : LOW); digitalWrite(selpin_B, (sensornumber & 2) > 0 ? HIGH : LOW); clearSignal(); - // Wait to allow signal to be read + delay(25); // Wait one cycle a little extra to collect signal. delay(200); } @@ -78,6 +86,13 @@ bool BWFSENSOR::isOutside() { return (signal_status == OUTSIDE); } +bool BWFSENSOR::isOutOfBounds() { + if (BWF_DETECTION_ALWAYS) + return !isInside(); + else + return isOutside(); +} + bool BWFSENSOR::isTimedOut() { return (last_match + TIMEOUT_DELAY < millis()); } diff --git a/Liam/BWFSensor.h b/Liam/BWFSensor.h index 0b8d800..361aff3 100644 --- a/Liam/BWFSensor.h +++ b/Liam/BWFSensor.h @@ -32,6 +32,7 @@ class BWFSENSOR { bool isInside(); bool isOutside(); bool isTimedOut(); + bool isOutOfBounds(); bool hasNoSignal(); void readSensor(); diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index 5ac358c..a9ab3ed 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -36,6 +36,16 @@ boolean CONTROLLER::allSensorsAreOutside() { return true; } +int CONTROLLER::getFirstSensorOutOfBounds() { + for (int i = 0; iselect(i); + if (sensor->isOutOfBounds()) + return i; + } + + return -1; +} + int CONTROLLER::turnToReleaseLeft(int angle) { turnLeft(angle); @@ -192,31 +202,34 @@ void CONTROLLER::setDefaultDirectionForward(bool fwd) { }; void CONTROLLER::adjustMotorSpeeds() { - int lms = abs(leftMotor->getSpeed()); - int rms = abs(rightMotor->getSpeed()); - - if (!sensor->isInside()) { - lms = 100; - rms = 10; + int lms; + int rms; + int ltime; + int rtime; + int lowSpeed = 40; + int highSpeed = FULLSPEED; + int shortTime = 10; + int longTime = 500; + + if (sensor->isOutOfBounds()) { + //Serial.println("Adjust to out of bounds"); + lms = highSpeed; + ltime = shortTime; + rms = lowSpeed; + rtime = longTime; } else - if (sensor->isInside()) - { - lms = 10; - rms = 100; - } - else { - rms += 80; - lms += 80; - } + { + //Serial.println("Adjust to inside bounds"); + lms = lowSpeed; + ltime = longTime; + rms = highSpeed; + rtime = shortTime; + } - if (rms > 100) rms = 100; - if (lms > 100) lms = 100; - if (rms < -50) rms = -50; - if (lms < -50) lms = -50; - leftMotor->setSpeed(default_dir_fwd*lms); - rightMotor->setSpeed(default_dir_fwd*rms); + leftMotor->setSpeedOverTime(default_dir_fwd*lms, ltime); + rightMotor->setSpeedOverTime(default_dir_fwd*rms, rtime); } void CONTROLLER::updateBalance() { @@ -256,13 +269,56 @@ int CONTROLLER::compensateSpeedToCompassHeading() { } boolean CONTROLLER::wheelsAreOverloaded() { - delay(200); // Settle current spikes - if (leftMotor->isOverloaded() | rightMotor->isOverloaded()) - return true; - else - return false; + long now = millis(); + int l_load = 0; + int r_load = 0; + int l_load_limit = 0; + int r_load_limit = 0; + int counter = 0; + while (millis() - now <= 200) + { + l_load = leftMotor->getLoad(); + l_load_limit = WHEELMOTOR_OVERLOAD * max(30,abs(leftMotor->getSpeed())) / FULLSPEED; + + r_load = rightMotor->getLoad(); + r_load_limit = WHEELMOTOR_OVERLOAD * max(30,abs(rightMotor->getSpeed())) / FULLSPEED; + /*counter++;*/ + delay(1); + if (l_load < l_load_limit && r_load < r_load_limit) + { + return false; + } + }; + + return true; } +void CONTROLLER::turnIfObstacle() { + // Check if bumper has triggered (providing you have one enabled) + if ( +#if defined __Bumper__ + hasBumped() || +#endif +#if defined __MS9150__ || defined __MS5883L__ + hasTilted() || +#endif + wheelsAreOverloaded()) { + int angle = random(90, 160); + runBackward(FULLSPEED); + + if (waitWhileInside(1200) == 0) { + + if (random(0, 100) % 2 == 0) { + turnRight(angle); + } + else { + turnLeft(angle); + } + compass->setNewTargetHeading(); + runForward(FULLSPEED); + } + } +} boolean CONTROLLER::hasBumped() { return !digitalRead(BUMPER); } diff --git a/Liam/Controller.h b/Liam/Controller.h index a37ff5a..7a3415d 100644 --- a/Liam/Controller.h +++ b/Liam/Controller.h @@ -33,8 +33,10 @@ class CONTROLLER { void runForward(int speed); void runBackward(int speed); void stop(); + void turnIfObstacle(); boolean allSensorsAreOutside(); + int getFirstSensorOutOfBounds(); void startCutter(); void stopCutter(); diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 04e9010..fd7a755 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -66,6 +66,10 @@ int state; long time_at_turning = millis(); int turn_direction = 1; int LCDi = 0; +int dockCollisionCount = 0; +long lastCollision = 0; +long dockingInsideSince = 0; +long lastDockingAllOutsideCheck = 0; // Set up all the defaults (check the Definition.h file for all default values) @@ -167,7 +171,7 @@ void loop() boolean mower_is_outside; int err=0; LCDi++; //Loops 0-10 - if (LCDi % 25 == 0 ){ + if (LCDi % 2500 == 0 ){ Display.update(); } @@ -218,12 +222,15 @@ void loop() Battery.updateVoltage(); Display.update(); + if (Battery.mustCharge()) { + state = LOOKING_FOR_BWF; + break; + } + Sensor.select(0); - if (BWF_DETECTION_ALWAYS) - mower_is_outside = !Sensor.isInside(); - else - mower_is_outside = Sensor.isOutside(); + mower_is_outside = Sensor.isOutOfBounds(); + // Check left sensor (0) and turn right if needed if (mower_is_outside) { @@ -235,15 +242,7 @@ void loop() if(err) Error.flag(err); #endif - if (Battery.mustCharge()) { - Mower.stopCutter(); - Mower.runForward(FULLSPEED); - delay(1000); - Mower.stop(); - Sensor.select(0); - state = DOCKING; - break; - } + // Tries to turn, but if timeout then reverse and try again if (err = Mower.turnToReleaseRight(30) > 0) { @@ -267,10 +266,7 @@ void loop() Sensor.select(1); - if (BWF_DETECTION_ALWAYS) - mower_is_outside = !Sensor.isInside(); - else - mower_is_outside = Sensor.isOutside(); + mower_is_outside = Sensor.isOutOfBounds(); // Check right sensor (1) and turn left if needed if (mower_is_outside) { @@ -314,29 +310,18 @@ void loop() Compass.updateHeading(); Mower.compensateSpeedToCompassHeading(); - // Check if mower has hit something - if (Mower.wheelsAreOverloaded()) - { - Serial.print("Wheel overload "); - Mower.runBackward(FULLSPEED); - if(Mower.waitWhileInside(2000) == 0); - Mower.turnRight(90); - Compass.setNewTargetHeading(); - Mower.runForward(FULLSPEED); - } + //if (Mower.wheelsAreOverloaded()) + //{ + // Serial.print("Wheel overload "); + // Mower.runBackward(FULLSPEED); + // if(Mower.waitWhileInside(2000) == 0); + // Mower.turnRight(90); + // Compass.setNewTargetHeading(); + // Mower.runForward(FULLSPEED); + //} + - // Check if bumper has triggered (providing you have one enabled) -#if defined __Bumper__ - if (Mower.hasBumped()) - { - Serial.print("Mower has bumped "); - Mower.runBackward(FULLSPEED); - delay(2000); - Mower.turnRight(90); - Mower.runForward(FULLSPEED); - } -#endif #if defined __Lift_Sensor__ if (Mower.isLifted()) @@ -355,15 +340,7 @@ void loop() // Check if mower has tilted (providing you have one enabled) #if defined __MS9150__ || defined __MS5883L__ - if (Mower.hasTilted()) - { - Serial.print("Mower has tilted "); - Mower.runBackward(FULLSPEED); - delay(2000); - Mower.turnRight(90); - Mower.runForward(FULLSPEED); - delay(200); - } + // Check if mower has flipped (providing you have one enabled) if (Mower.hasFlipped()) @@ -374,6 +351,7 @@ void loop() Error.flag(ERROR_TILT); } #endif + Mower.turnIfObstacle(); break; @@ -382,6 +360,11 @@ void loop() //----------------------- LAUNCHING --------------------------- case LAUNCHING: + //Don't launch without signal + if (!Sensor.isInside() && Sensor.isOutside()) { + break; + } + Mower.runBackward(FULLSPEED); delay(7000); @@ -406,37 +389,108 @@ void loop() //----------------------- DOCKING ----------------------------- case DOCKING: - //Make the wheel motors extra responsive - leftMotor.setSmoothness(10); - rightMotor.setSmoothness(10); + if (Battery.isBeingCharged()) { + // Stop + Mower.stop(); + Mower.resetBalance(); + state = CHARGING; + break; + } + + if (millis() - lastCollision > 10000) { + dockCollisionCount = 0; + } // If the mower hits something, reverse and try again - if (Mower.wheelsAreOverloaded()){ + if (Mower.wheelsAreOverloaded()){ + dockCollisionCount++; + lastCollision = millis(); + + Serial.println(dockCollisionCount); + + //Pause to let charge starting current pan out. + delay(1000); + if (Battery.isBeingCharged()) { + Mower.stop(); + Mower.resetBalance(); + state = CHARGING; + break; + } Mower.runBackward(FULLSPEED); - delay(1000); - } + delay(1300); + Mower.stop(); + + if (dockCollisionCount > 2) { + Mower.turnRight(70); + Mower.stop(); + dockCollisionCount = 0; + dockingInsideSince =0; + Mower.runForward(FULLSPEED); + break; + } + + Mower.runForward(FULLSPEED); + } + + if (millis() - lastDockingAllOutsideCheck > 500) { + //Serial.println("Check for for right outside"); + Sensor.select(1); + if (Sensor.isOutOfBounds()) { + Mower.stop(); + Mower.runBackward(FULLSPEED); + delay(700); + Mower.stop(); + Mower.turnRight(30); + Mower.stop(); + Mower.runForward(FULLSPEED); + } + lastDockingAllOutsideCheck = millis(); + + } + + + + Sensor.select(0); + if (!Sensor.isOutOfBounds()) { + if (dockingInsideSince == 0) { + dockingInsideSince = millis(); + } + else if (millis() - dockingInsideSince > 10000){ + + Mower.stop(); + Mower.turnLeft(30); + dockingInsideSince = 0; + state = LOOKING_FOR_BWF; + break; + } + } + else { + dockingInsideSince = 0; + //Serial.println("Out"); + } + // Track the BWF by compensating the wheel motor speeds Mower.adjustMotorSpeeds(); // Clear signal to allow the mower to track the wire closely - Sensor.clearSignal(); - - // Wait a little to avoid current spikes - delay(100); - - // Stop the mower as soon as the charge plates come in contact - if (Battery.isBeingCharged()) { - // Stop - Mower.stop(); - Mower.resetBalance(); - state = CHARGING; - break; - } - + //Sensor.clearSignal(); break; + case LOOKING_FOR_BWF: + Mower.stopCutter(); + Sensor.select(0); + + if (Sensor.isOutOfBounds()) { + Serial.println("BWF found"); + state = DOCKING; + } + else { + Mower.runForward(FULLSPEED); + Mower.turnIfObstacle(); + } + break; //----------------------- CHARGING ---------------------------- case CHARGING: // restore wheelmotor smoothness diff --git a/Liam/Wheelmotor.cpp b/Liam/Wheelmotor.cpp index da76e0d..bf801ec 100644 --- a/Liam/Wheelmotor.cpp +++ b/Liam/Wheelmotor.cpp @@ -19,9 +19,47 @@ WHEELMOTOR::WHEELMOTOR(int pwmpin_, int dirpin_, int loadpin_, int smoothness) { } + + +void WHEELMOTOR::setSpeedOverTime(int speed, int actionTime) { + //void setPWM(int pwmPin, int directionPin, int targetValue, int totalActionMs) { + + + int _now = millis(); + if (speed != ot_currentTargetValue) { + ot_currentTargetValue = speed; + ot_startingValue = ot_currentValue; + ot_setTime = _now; + } + + if (speed == ot_currentValue) { + return; + } + + + int newValue; + if (actionTime == 0) { + newValue = speed; + } if (ot_setTime + actionTime < _now) { + newValue = speed; + } + else { + newValue = map(_now, ot_setTime, ot_setTime + actionTime, ot_startingValue, speed); + } + + analogWrite(pwmpin, 2.55*abs(newValue)); + digitalWrite(dirpin, (newValue > 0)); + + //Serial.println(newValue); + speed = newValue; + ot_currentValue = newValue; + +} + void WHEELMOTOR::setSpeed(int setspeed) { - if (setspeed > 100) setspeed = 100; - if (setspeed < -100) setspeed = -100; + ot_startingValue = setspeed; + if (setspeed > 100) setspeed = 100; + if (setspeed < -100) setspeed = -100; // Increase or decrease speed? int diff = (setspeed < speed)? -1 : 1; @@ -29,10 +67,10 @@ void WHEELMOTOR::setSpeed(int setspeed) { // Ramp up/down motor smoothly by changing speed by one %-unit at a time. while(speed != setspeed) { - speed += diff; + speed += diff; analogWrite(pwmpin, 255*abs(speed)/100); - digitalWrite(dirpin, (speed > 0)); + digitalWrite(dirpin, (speed > 0)); delayMicroseconds(smoothness_delay); } diff --git a/Liam/Wheelmotor.h b/Liam/Wheelmotor.h index 5e44b05..6e69466 100644 --- a/Liam/Wheelmotor.h +++ b/Liam/Wheelmotor.h @@ -23,6 +23,8 @@ class WHEELMOTOR { void setSpeed(int setspeed); int getSpeed(); + void setSpeedOverTime(int speed, int actionTime); + int getLoad(); bool isOverloaded(); @@ -37,6 +39,11 @@ class WHEELMOTOR { int speed; int overload_level; int smoothness_delay; + + int ot_setTime; + int ot_currentTargetValue; + int ot_startingValue; + int ot_currentValue; }; #endif /* _WHEELMOTOR_H_ */ From 4f64548c827a0d7d6194accebfebbc5b97d29bc3 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Wed, 30 May 2018 08:55:37 +0200 Subject: [PATCH 12/28] Missing constant --- Liam/Definition.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Liam/Definition.h b/Liam/Definition.h index 01d4b48..4e8341a 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -63,6 +63,7 @@ const int MOWING = 0; const int LAUNCHING = 1; const int DOCKING = 2; const int CHARGING = 3; +const int LOOKING_FOR_BWF = 4; // Turning details #define TURN_INTERVAL 15000 From a7e2428a331c306071d25180e028b9a9692ac11f Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Thu, 31 May 2018 18:16:43 +0200 Subject: [PATCH 13/28] On the fly debug mode --- Liam/Definition.h | 1 + Liam/Liam.ino | 26 +++++++++++++++++++------- Liam/SetupDebug.cpp | 31 ++++++++++++++++++++++++++++--- Liam/SetupDebug.h | 6 ++++-- 4 files changed, 52 insertions(+), 12 deletions(-) diff --git a/Liam/Definition.h b/Liam/Definition.h index fdd9168..ffecd40 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -64,6 +64,7 @@ const int LAUNCHING = 1; const int DOCKING = 2; const int CHARGING = 3; const int LOOKING_FOR_BWF = 4; +const int SETUP_DEBUG = 5; // Turning details #define TURN_INTERVAL 15000 diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 63c69ae..49da5c0 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -60,6 +60,7 @@ #include "Sens5883L.h" #include "Sens9150.h" #include "Definition.h" +#include "SetupDebug.h" // Global variables int state; @@ -104,6 +105,8 @@ MOTIONSENSOR Compass; // Controller (pass adresses to the motors and sensors for the controller to operate on) CONTROLLER Mower(&leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass); +SETUPDEBUG SetDeb(&Mower, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &Battery); + // Display #if defined __LCD__ myLCD Display(&Battery, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &state); @@ -150,14 +153,19 @@ void setup() Display.print(__DATE__ " " __TIME__ "\n"); delay(5000); Display.clear(); + SetDeb.initialize(&Serial); + state = SetDeb.tryEnterSetupDebugMode(0); - if (Battery.isBeingCharged()) { // If Liam is in docking station then - state = CHARGING; // continue charging - Mower.stopCutter(); - } else { // otherwise - state = MOWING; - Mower.startCutter(); // Start up the cutter motor - Mower.runForward(FULLSPEED); + if (state != SETUP_DEBUG) { + if (Battery.isBeingCharged()) { // If Liam is in docking station then + state = CHARGING; // continue charging + Mower.stopCutter(); + } + else { // otherwise + state = MOWING; + Mower.startCutter(); // Start up the cutter motor + Mower.runForward(FULLSPEED); + } } } @@ -167,6 +175,10 @@ void setup() // ***************** Main loop *********************************** void loop() { + state = SetDeb.tryEnterSetupDebugMode(state); + if (state == SETUP_DEBUG) { + return; + } long looptime= millis(); boolean in_contact; boolean mower_is_outside; diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index 8d21435..2ffc62a 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -11,7 +11,8 @@ #include "SetupDebug.h" #include "Definition.h" -SETUPDEBUG::SETUPDEBUG(WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt) { +SETUPDEBUG::SETUPDEBUG(CONTROLLER* controller, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt) { + mower = controller; leftMotor = left; rightMotor = right; cutter = cut; @@ -29,9 +30,24 @@ void SETUPDEBUG::initialize(HardwareSerial *serIn) { _Serial->println("Send 'H' for list of commands"); } -void SETUPDEBUG::startListeningOnSerial() { +int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { char inChar; + + + if (currentState != SETUP_DEBUG) { + if (Serial.available()) { + inChar = (char)Serial.read(); + if (inChar == 'd' || inChar == 'D') { + mower->stopCutter(); + mower->stop(); + SETUPDEBUG::printHelp(); + return SETUP_DEBUG; + } + } + return currentState; + }; + while (!Serial.available()); // Stay here until data is available inChar = (char)Serial.read(); // get the new byte: @@ -179,6 +195,13 @@ void SETUPDEBUG::startListeningOnSerial() { // target_heading = current_heading; break; + case 'm': + case 'M': + mower->runForward(FULLSPEED); + return MOWING; + case 'b': + case 'B': + return LOOKING_FOR_BWF; } // case 'g': @@ -192,7 +215,7 @@ void SETUPDEBUG::startListeningOnSerial() { inChar = 0; - +return SETUP_DEBUG; } void SETUPDEBUG::printHelp() { @@ -206,6 +229,8 @@ void SETUPDEBUG::printHelp() { _Serial->println("T = make a 10 second test run"); _Serial->println("P = print battery & debug values"); _Serial->println("E = Cutter motor calibrate"); + _Serial->println("M = Start mowing"); + _Serial->println("B = Look for BWF and dock"); } void SETUPDEBUG::toggleWheelLeft() { diff --git a/Liam/SetupDebug.h b/Liam/SetupDebug.h index 1935cf4..795072e 100644 --- a/Liam/SetupDebug.h +++ b/Liam/SetupDebug.h @@ -14,17 +14,19 @@ #include "BWFSensor.h" #include "MotionSensor.h" #include "Definition.h" +#include "Controller.h" #ifndef _SETUPDEBUG_H_ #define _SETUPDEBUG_H_ class SETUPDEBUG { public: - SETUPDEBUG(WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt); + SETUPDEBUG(CONTROLLER* mower, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt); void initialize(HardwareSerial *serIn); - void startListeningOnSerial(); + int tryEnterSetupDebugMode(int currentState); void updateBWF(); private: + CONTROLLER* mower; WHEELMOTOR* leftMotor; WHEELMOTOR* rightMotor; CUTTERMOTOR* cutter; From 885d15dc84342fc19bc3b8acb608826277c8469b Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Thu, 31 May 2018 19:13:30 +0200 Subject: [PATCH 14/28] Rename and startup message --- Liam/Liam.ino | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 49da5c0..b91eac9 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -105,7 +105,7 @@ MOTIONSENSOR Compass; // Controller (pass adresses to the motors and sensors for the controller to operate on) CONTROLLER Mower(&leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass); -SETUPDEBUG SetDeb(&Mower, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &Battery); +SETUPDEBUG SetupAndDebug(&Mower, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &Battery); // Display #if defined __LCD__ @@ -151,10 +151,12 @@ void setup() Display.print(F("--- LIAM ---\n")); Display.print(F(VERSION_STRING "\n")); Display.print(__DATE__ " " __TIME__ "\n"); + Serial.println("----------------"); + Serial.println("Send D to enter setup and debug mode"); delay(5000); Display.clear(); - SetDeb.initialize(&Serial); - state = SetDeb.tryEnterSetupDebugMode(0); + SetupAndDebug.initialize(&Serial); + state = SetupAndDebug.tryEnterSetupDebugMode(0); if (state != SETUP_DEBUG) { if (Battery.isBeingCharged()) { // If Liam is in docking station then @@ -175,7 +177,7 @@ void setup() // ***************** Main loop *********************************** void loop() { - state = SetDeb.tryEnterSetupDebugMode(state); + state = SetupAndDebug.tryEnterSetupDebugMode(state); if (state == SETUP_DEBUG) { return; } From bb59828a830e49a1186cccf8c8263c46368d9898 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Sat, 2 Jun 2018 20:39:37 +0200 Subject: [PATCH 15/28] Missing signal should result in NOSIGNAL --- Liam/BWFSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Liam/BWFSensor.cpp b/Liam/BWFSensor.cpp index 2252db5..1ac6ffb 100644 --- a/Liam/BWFSensor.cpp +++ b/Liam/BWFSensor.cpp @@ -68,7 +68,7 @@ void BWFSENSOR::select(int sensornumber) { { if (millis() - time >= BWF_COLLECT_SIGNAL_TIME) // max time of 200ms { - signal_status = OUTSIDE; + signal_status = NOSIGNAL; } } // delay(200); From 982f36bb6af8bac0711206b9324b72917e1c4d14 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Mon, 4 Jun 2018 20:07:35 +0200 Subject: [PATCH 16/28] Possible hang fix --- .gitignore | 1 + Liam/BWFSensor.cpp | 21 ++++++++++----------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/.gitignore b/.gitignore index 97837fc..e962d01 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ *.sln *.vcxproj* *.orig +*/Release/* diff --git a/Liam/BWFSensor.cpp b/Liam/BWFSensor.cpp index 1ac6ffb..995c3a2 100644 --- a/Liam/BWFSensor.cpp +++ b/Liam/BWFSensor.cpp @@ -54,23 +54,22 @@ BWFSENSOR::BWFSENSOR(int selA, int selB) { // Select active sensor void BWFSENSOR::select(int sensornumber) { - if (currentSensor == sensornumber) { - - return; - } - currentSensor = sensornumber; - + if (currentSensor == sensornumber) { + + return; + } + currentSensor = sensornumber; + digitalWrite(selpin_A, (sensornumber & 1) > 0 ? HIGH : LOW); digitalWrite(selpin_B, (sensornumber & 2) > 0 ? HIGH : LOW); clearSignal(); long time = millis(); - while (!gotSignal()) + while (signal_status == NOSIGNAL + && millis() - time < BWF_COLLECT_SIGNAL_TIME) // max time of 200ms { - if (millis() - time >= BWF_COLLECT_SIGNAL_TIME) // max time of 200ms - { - signal_status = NOSIGNAL; - } + delay(1); } + // delay(200); } From 435165e42c3317bc74e7611d7f6656ccb4ba5ab0 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Mon, 4 Jun 2018 22:42:22 +0200 Subject: [PATCH 17/28] SetupDebog uses global Serial object. Possible to deactivate SetupDebug from definition.h --- Liam/Definition.h | 4 +++- Liam/Liam.ino | 20 ++++++++++++++++---- Liam/SetupDebug.cpp | 32 ++++++++++++-------------------- Liam/SetupDebug.h | 2 -- 4 files changed, 31 insertions(+), 27 deletions(-) diff --git a/Liam/Definition.h b/Liam/Definition.h index ffecd40..4477fd7 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -16,6 +16,8 @@ #ifndef _DEFINITION_H_ #define _DEFINITION_H_ +#define DEBUG_ENABLED true + const int NUMBER_OF_SENSORS = 2; // Number of BWF sensors can be 1-4 depending on shield default 2 left and right front. // Pin setup following Morgan 1.5 shield and up @@ -74,7 +76,7 @@ const int SETUP_DEBUG = 5; #pragma region BWF // BWF Detection method (true = always, false = only at wire) -#define BWF_DETECTION_ALWAYS true +#define BWF_DETECTION_ALWAYS false #define TIMEOUT 6000 //Time without signal before error #define BWF_COLLECT_SIGNAL_TIME 200 // max time to spend looking for signal #define BWF_NUMBER_OF_PULSES 3 diff --git a/Liam/Liam.ino b/Liam/Liam.ino index b91eac9..210702c 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -60,7 +60,10 @@ #include "Sens5883L.h" #include "Sens9150.h" #include "Definition.h" -#include "SetupDebug.h" + +#ifdef DEBUG_ENABLED + #include "SetupDebug.h" +#endif // Global variables int state; @@ -105,7 +108,9 @@ MOTIONSENSOR Compass; // Controller (pass adresses to the motors and sensors for the controller to operate on) CONTROLLER Mower(&leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass); +#ifdef DEBUG_ENABLED SETUPDEBUG SetupAndDebug(&Mower, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &Battery); +#endif // Display #if defined __LCD__ @@ -151,12 +156,16 @@ void setup() Display.print(F("--- LIAM ---\n")); Display.print(F(VERSION_STRING "\n")); Display.print(__DATE__ " " __TIME__ "\n"); + + #ifdef DEBUG_ENABLED Serial.println("----------------"); Serial.println("Send D to enter setup and debug mode"); + //SetupAndDebug.initialize(&Serial); + state = SetupAndDebug.tryEnterSetupDebugMode(0); + #endif + delay(5000); Display.clear(); - SetupAndDebug.initialize(&Serial); - state = SetupAndDebug.tryEnterSetupDebugMode(0); if (state != SETUP_DEBUG) { if (Battery.isBeingCharged()) { // If Liam is in docking station then @@ -177,10 +186,13 @@ void setup() // ***************** Main loop *********************************** void loop() { + + #ifdef DEBUG_ENABLED state = SetupAndDebug.tryEnterSetupDebugMode(state); if (state == SETUP_DEBUG) { return; } + #endif long looptime= millis(); boolean in_contact; boolean mower_is_outside; @@ -455,7 +467,7 @@ void loop() Mower.runBackward(FULLSPEED); delay(700); Mower.stop(); - Mower.turnRight(30); + Mower.turnRight(20); Mower.stop(); Mower.runForward(FULLSPEED); } diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index 2ffc62a..17a9b73 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -19,17 +19,9 @@ SETUPDEBUG::SETUPDEBUG(CONTROLLER* controller, WHEELMOTOR* left, WHEELMOTOR* rig sensor = bwf; compass = comp; battery = batt; - _Serial = NULL; pitch = 0; } -void SETUPDEBUG::initialize(HardwareSerial *serIn) { - _Serial = serIn; - _Serial->begin(115200); - _Serial->println("Welcome to Liam Test Program"); - _Serial->println("Send 'H' for list of commands"); -} - int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { char inChar; @@ -219,18 +211,18 @@ return SETUP_DEBUG; } void SETUPDEBUG::printHelp() { - _Serial->println("------- Help menu ------------"); - _Serial->println("L = Left Wheel motor on/off"); - _Serial->println("R = Right Wheel motor on/off"); - _Serial->println("C = Cutter motor on/off"); - _Serial->println("S = test BWF Sensor"); - _Serial->println("G = test Gyro/Compass/Accelerometer"); - _Serial->println("D = turn LED on/off"); - _Serial->println("T = make a 10 second test run"); - _Serial->println("P = print battery & debug values"); - _Serial->println("E = Cutter motor calibrate"); - _Serial->println("M = Start mowing"); - _Serial->println("B = Look for BWF and dock"); + Serial.println("------- Help menu ------------"); + Serial.println("L = Left Wheel motor on/off"); + Serial.println("R = Right Wheel motor on/off"); + Serial.println("C = Cutter motor on/off"); + Serial.println("S = test BWF Sensor"); + Serial.println("G = test Gyro/Compass/Accelerometer"); + Serial.println("D = turn LED on/off"); + Serial.println("T = make a 10 second test run"); + Serial.println("P = print battery & debug values"); + Serial.println("E = Cutter motor calibrate"); + Serial.println("M = Start mowing"); + Serial.println("B = Look for BWF and dock"); } void SETUPDEBUG::toggleWheelLeft() { diff --git a/Liam/SetupDebug.h b/Liam/SetupDebug.h index 795072e..0f22f73 100644 --- a/Liam/SetupDebug.h +++ b/Liam/SetupDebug.h @@ -22,7 +22,6 @@ class SETUPDEBUG { public: SETUPDEBUG(CONTROLLER* mower, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt); - void initialize(HardwareSerial *serIn); int tryEnterSetupDebugMode(int currentState); void updateBWF(); private: @@ -33,7 +32,6 @@ class SETUPDEBUG { BWFSENSOR* sensor; MOTIONSENSOR* compass; BATTERY* battery; - HardwareSerial *_Serial; void printHelp(); void toggleWheelLeft(); void togglewheelRight(); From 525243dc71197dc52e1af6e8d099cb86be8750d5 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Tue, 5 Jun 2018 22:03:29 +0200 Subject: [PATCH 18/28] Added 90 degrees turn to SetupDebug to help setting TURNDELAY --- Liam/SetupDebug.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index 17a9b73..c0bba4f 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -147,6 +147,11 @@ int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { break; + case '9': + mower->turnRight(90); + mower->stop(); + Serial.print("If turn was not 90 degrees, consider altering TURNDELAY in definition.h"); + break; case 'g': case 'G': #if __MMA7455__ @@ -214,6 +219,7 @@ void SETUPDEBUG::printHelp() { Serial.println("------- Help menu ------------"); Serial.println("L = Left Wheel motor on/off"); Serial.println("R = Right Wheel motor on/off"); + Serial.println("9 = Turn 90 degrees right"); Serial.println("C = Cutter motor on/off"); Serial.println("S = test BWF Sensor"); Serial.println("G = test Gyro/Compass/Accelerometer"); From ea4a2600a0fc89bb9a8cde2484f2b007c96362a4 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Tue, 5 Jun 2018 22:05:13 +0200 Subject: [PATCH 19/28] waitWhileInside uses isOutOfBounds instead of isInside --- Liam/Controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index bd5c645..1cd5d17 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -139,7 +139,7 @@ int CONTROLLER::waitWhileInside(int duration) { for (int k=0; kselect(i); - if(!sensor->isInside()) + if(sensor->isOutOfBounds()) return 2; } From fbfbfff406b5e9720c2366343394a388d16262e9 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Tue, 5 Jun 2018 22:07:45 +0200 Subject: [PATCH 20/28] Don't abort backing up --- Liam/Controller.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index 1cd5d17..382fa94 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -305,18 +305,16 @@ void CONTROLLER::turnIfObstacle() { wheelsAreOverloaded()) { int angle = random(90, 160); runBackward(FULLSPEED); + delay(1200); - if (waitWhileInside(1200) == 0) { - - if (random(0, 100) % 2 == 0) { - turnRight(angle); - } - else { - turnLeft(angle); - } - compass->setNewTargetHeading(); - runForward(FULLSPEED); + if (random(0, 100) % 2 == 0) { + turnRight(angle); + } + else { + turnLeft(angle); } + compass->setNewTargetHeading(); + runForward(FULLSPEED); } } boolean CONTROLLER::hasBumped() { From 9d0f314b05cd913dc462503530a22cb51417af8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Tue, 5 Jun 2018 21:36:06 +0200 Subject: [PATCH 21/28] Add support for local definitions - #defines made in the file Liam/LocalDefinition.h will override those made in Liam/Definition.h - Liam/LocalDefinition.h is ignored by git, and will remain unchanged when pulling down updated code - Added CUTTER_MOTOR_TYPE and BATTERY_TYPE macros to Definition.h and moved them to Definition.h --- .gitignore | 1 + Liam/Definition.h | 41 +++++++++++++++++++++++++++-------------- Liam/Liam.ino | 8 ++------ README.md | 6 ++++++ 4 files changed, 36 insertions(+), 20 deletions(-) diff --git a/.gitignore b/.gitignore index e962d01..0d98991 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +Liam/LocalDefinition.h .DS_Store .vscode */.vs/ diff --git a/Liam/Definition.h b/Liam/Definition.h index 4477fd7..07d7a39 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -8,6 +8,11 @@ ====================== */ +/* + Note that settings in this file can be overridden in a file + named `LocalDefinition.h` which is not tracked by git. +*/ + #include #include "Wheelmotor.h" #include "CutterMotor.h" @@ -16,6 +21,21 @@ #ifndef _DEFINITION_H_ #define _DEFINITION_H_ + +// Cutter motor types +#define BRUSHLESS 0 +#define BRUSHED 1 +#define NIDEC 2 + +#define CUTTER_MOTOR_TYPE BRUSHED + +// Battery +#define LEAD_ACID 0 +#define NIMH 1 +#define LIION 2 + +#define BATTERY_TYPE LIION + #define DEBUG_ENABLED true const int NUMBER_OF_SENSORS = 2; // Number of BWF sensors can be 1-4 depending on shield default 2 left and right front. @@ -43,15 +63,6 @@ const int NUMBER_OF_SENSORS = 2; // Number of BWF sensors can be 1-4 depending #define WHEEL_MOTOR_A_DIRECTION_PIN 12 #define WHEEL_MOTOR_B_DIRECTION_PIN 13 -// Cutter motor types -#define BRUSHLESS 0 -#define BRUSHED 1 -#define NIDEC 2 - -// Battery -#define LEAD_ACID 0 -#define NIMH 1 -#define LIION 2 // Wheel motor #define WHEELMOTOR_OVERLOAD 130 @@ -84,11 +95,7 @@ const int SETUP_DEBUG = 5; // The higher value the more turns (in the same direction) the mower can make before leaving #define BALANCE_TRIGGER_LEVEL 10000 -// Code for inside and outside -//#define INSIDE_BWF 103,4,103 -//#define OUTSIDE_BWF 103,107,103 - -// Version 2 of the BWF transmitter +// BWF Code for inside and outside #define INSIDE_BWF 85 #define OUTSIDE_BWF 5 @@ -150,4 +157,10 @@ class DEFINITION { private: }; + +// Include LocalDefinition.h if it exists +#if __has_include("LocalDefinition.h") +#include "LocalDefinition.h" +#endif + #endif /* _DEFINITION_H_ */ diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 210702c..c9c8f07 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -80,18 +80,14 @@ long lastDockingAllOutsideCheck = 0; DEFINITION Defaults; // Please select which type of cutter motor you have -// Types available: BRUSHED (for all brushed motors, A3620 and NIDEC 22) -// BRUSHLESS (for all hobbyking motors with external ESC) -// NIDEC (for NIDEC 24) -CUTTERMOTOR CutterMotor(BRUSHED, CUTTER_PWM_PIN, CUTTER_CURRENT_PIN); +CUTTERMOTOR CutterMotor(CUTTER_MOTOR_TYPE, CUTTER_PWM_PIN, CUTTER_CURRENT_PIN); // Wheelmotors WHEELMOTOR rightMotor(WHEEL_MOTOR_A_PWM_PIN, WHEEL_MOTOR_A_DIRECTION_PIN, WHEEL_MOTOR_A_CURRENT_PIN, WHEELMOTOR_SMOOTHNESS); WHEELMOTOR leftMotor(WHEEL_MOTOR_B_PWM_PIN, WHEEL_MOTOR_B_DIRECTION_PIN, WHEEL_MOTOR_B_CURRENT_PIN, WHEELMOTOR_SMOOTHNESS); // Battery -// Battery types available are LIION, LEAD_ACID, NIMH -BATTERY Battery(LIION, BAT_PIN, DOCK_PIN); +BATTERY Battery(BATTERY_TYPE, BAT_PIN, DOCK_PIN); // BWF Sensors BWFSENSOR Sensor(BWF_SELECT_B_PIN, BWF_SELECT_A_PIN); diff --git a/README.md b/README.md index a940f34..72bd3db 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,12 @@ Most of the default values for your mower can be set in the Definition.h file. Use this to configure your mower for best performance. +Configurations can also be put in a file called +`LocalDefinition.h`, which will override `Definition.h` but +is not tracked by git. This might be useful if you like to +stay on top of any code updates, but don't want to remake +the same configuration changes all the time. + Compile ------ From 68116163f97e00ca08c79b7dc7c5178b416a4f12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Thu, 24 May 2018 21:35:38 +0200 Subject: [PATCH 22/28] Cleanup of SetupDebug - Added feature to start debug mode by sending D over serial port when mower is starting up - Removed unused functions and variables in SetupDebug - Split remaining debug command out into separate functions --- Liam/Liam.ino | 11 +- Liam/SetupDebug.cpp | 281 ++++++++++++++++++++------------------------ Liam/SetupDebug.h | 20 ++-- 3 files changed, 144 insertions(+), 168 deletions(-) diff --git a/Liam/Liam.ino b/Liam/Liam.ino index c9c8f07..0433d39 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -75,6 +75,8 @@ long lastCollision = 0; long dockingInsideSince = 0; long lastDockingAllOutsideCheck = 0; +bool debug_mode = false; + // Set up all the defaults (check the Definition.h file for all default values) DEFINITION Defaults; @@ -128,6 +130,7 @@ void updateBWF() { Sensor.readSensor(); } + // ****************** Setup ************************************** void setup() { @@ -152,10 +155,10 @@ void setup() Display.print(F("--- LIAM ---\n")); Display.print(F(VERSION_STRING "\n")); Display.print(__DATE__ " " __TIME__ "\n"); - + #ifdef DEBUG_ENABLED - Serial.println("----------------"); - Serial.println("Send D to enter setup and debug mode"); + Serial.println(F("----------------")); + Serial.println(F("Send D to enter setup and debug mode")); //SetupAndDebug.initialize(&Serial); state = SetupAndDebug.tryEnterSetupDebugMode(0); #endif @@ -182,7 +185,7 @@ void setup() // ***************** Main loop *********************************** void loop() { - + #ifdef DEBUG_ENABLED state = SetupAndDebug.tryEnterSetupDebugMode(state); if (state == SETUP_DEBUG) { diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index c0bba4f..489a242 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -19,14 +19,12 @@ SETUPDEBUG::SETUPDEBUG(CONTROLLER* controller, WHEELMOTOR* left, WHEELMOTOR* rig sensor = bwf; compass = comp; battery = batt; - pitch = 0; } int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { char inChar; - if (currentState != SETUP_DEBUG) { if (Serial.available()) { inChar = (char)Serial.read(); @@ -38,7 +36,7 @@ int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { } } return currentState; - }; + } while (!Serial.available()); // Stay here until data is available inChar = (char)Serial.read(); // get the new byte: @@ -71,126 +69,33 @@ int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { case 'C': case 'c': - if (cutter_motor_is_on) { - Serial.println("Ramping down cutter"); - for (int i=100; i>=0; i--) { - cutter->setSpeed(i); - delay(10); - } - Serial.println("Ramp down completed"); - cutter_motor_is_on = false; - } - else - { - Serial.println("Ramping up cutter"); - for (int i=0; i<100; i++) { - cutter->setSpeed(i); - delay(10); - } - Serial.println("Ramp up completed"); - cutter_motor_is_on = true; - } + SETUPDEBUG::toggleCutterMotor(); break; case 'T': case 't': - for (int i=0; i<100; i++) { - sensor->select(0); - delay(100); - rightMotor->setSpeed((sensor->isInside()?100:-100)); - - sensor->select(1); - delay(100); - leftMotor->setSpeed((sensor->isInside()?100:-100)); - } - leftMotor->setSpeed(0); - rightMotor->setSpeed(0); + SETUPDEBUG::testRun(); break; case '+': - cutterspeed += 10; - cutter->setSpeed(cutterspeed); - Serial.println(cutterspeed); + SETUPDEBUG::cutterSpeedUp(); break; case '-': - cutterspeed -= 10; - cutter->setSpeed(cutterspeed); - Serial.println(cutterspeed); + SETUPDEBUG::cutterSpeedDown(); break; case 'P': case 'p': - Serial.print(" LMot: "); - Serial.print(leftMotor->getLoad()); - Serial.print(" RMot: "); - Serial.print(rightMotor->getLoad()); - Serial.print(" BAT: "); - battery->resetVoltage(); - Serial.print(battery->getVoltage()); - Serial.print(" Dock: "); - Serial.print(battery->isBeingCharged()); - - break; - - case 'e': - case 'E': - if (cutter_is_attached) { - // cutter->detachMotor(); - Serial.println("Cutter is detached"); - } - else { - // cutter.initialize(); - Serial.println("Cutter is attached"); - } - cutter_is_attached = !cutter_is_attached; - + SETUPDEBUG::printStatus(); break; case '9': - mower->turnRight(90); - mower->stop(); - Serial.print("If turn was not 90 degrees, consider altering TURNDELAY in definition.h"); + SETUPDEBUG::turnRight(); break; case 'g': case 'G': -#if __MMA7455__ - compass->autoupdate(); -#endif - - tilt_angle = compass->getTiltAngle(); - - x = compass->getXAngle(); - y = compass->getYAngle(); - z = compass->getZAngle(); - - Serial.print("Z = "); - Serial.println(z); - Serial.print("Y = "); - Serial.println(y); - Serial.print("X = "); - Serial.println(x); - - Serial.print("Tilt angle = "); - Serial.println(tilt_angle); - - // Compass.getHeading(); - // Compass.headingVsTarget(); - // Compass.updateHeading(); - - // my9150.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); - // Serial.print("TiltXZ: "); - // Serial.print(abs(atan2(ax,az))* 180 / M_PI); - // Serial.print(" TiltYZ: "); - // Serial.print(abs(atan2(ay,az))* 180 / M_PI); - // Serial.print("Heading: "); - // Serial.print(current_heading = atan2(my,mz)* 180 / M_PI); - // Serial.print("Diff to last heading: "); - // Serial.println(copysign(1.0,current_heading - target_heading) * - // copysign(1.0,abs(current_heading-target_heading)-180) * - // (180-abs(abs(current_heading-target_heading)-180))); - // target_heading = current_heading; - + SETUPDEBUG::getMotionValues(); break; case 'm': case 'M': @@ -201,110 +106,182 @@ int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { return LOOKING_FOR_BWF; } - // case 'g': - // case 'G': - // break; - // - // - // } - // Serial.println("P = print battery & debug values"); - // Serial.println("E = Cutter motor calibrate"); -inChar = 0; + inChar = 0; return SETUP_DEBUG; } void SETUPDEBUG::printHelp() { - Serial.println("------- Help menu ------------"); - Serial.println("L = Left Wheel motor on/off"); - Serial.println("R = Right Wheel motor on/off"); - Serial.println("9 = Turn 90 degrees right"); - Serial.println("C = Cutter motor on/off"); - Serial.println("S = test BWF Sensor"); - Serial.println("G = test Gyro/Compass/Accelerometer"); - Serial.println("D = turn LED on/off"); - Serial.println("T = make a 10 second test run"); - Serial.println("P = print battery & debug values"); - Serial.println("E = Cutter motor calibrate"); - Serial.println("M = Start mowing"); - Serial.println("B = Look for BWF and dock"); + Serial.println(F("------- Help menu ------------")); + Serial.println(F("L = Left Wheel motor on/off")); + Serial.println(F("R = Right Wheel motor on/off")); + Serial.println(F("9 = Turn 90 degrees right")); + Serial.println(F("C = Cutter motor on/off")); + Serial.println(F("S = test BWF Sensor")); + Serial.println(F("G = test Gyro/Compass/Accelerometer")); + Serial.println(F("D = turn LED on/off")); + Serial.println(F("T = make a 10 second test run")); + Serial.println(F("P = print battery & debug values")); + Serial.println(F("M = Start mowing")); + Serial.println(F("B = Look for BWF and dock")); +} + +void SETUPDEBUG::toggleLed() { + + if (led_is_on) + digitalWrite(10,LOW); + else + digitalWrite(10,HIGH); + + led_is_on = !led_is_on; } void SETUPDEBUG::toggleWheelLeft() { if (left_wheel_motor_is_on == true) { - Serial.println("Ramping down left wheel"); + Serial.println(F("Ramping down left wheel")); for (int i=100; i>0; i--) { leftMotor->setSpeed(i); delay(10); } - Serial.println("Ramp down completed"); + Serial.println(F("Ramp down completed")); left_wheel_motor_is_on = false; } else { - Serial.println("Ramping up left wheel"); + Serial.println(F("Ramping up left wheel")); for (int i=0; i<100; i++) { leftMotor->setSpeed(i); delay(10); } - Serial.println("Ramp up completed"); + Serial.println(F("Ramp up completed")); left_wheel_motor_is_on = true; } } void SETUPDEBUG::togglewheelRight() { if (right_wheel_motor_is_on == true) { - Serial.println("Ramping down right wheel"); + Serial.println(F("Ramping down right wheel")); for (int i=100; i>0; i--) { rightMotor->setSpeed(i); delay(10); } - Serial.println("Ramp down completed"); + Serial.println(F("Ramp down completed")); right_wheel_motor_is_on = false; } else { - Serial.println("Ramping up right wheel"); + Serial.println(F("Ramping up right wheel")); for (int i=0; i<100; i++) { rightMotor->setSpeed(i); delay(10); } - Serial.println("Ramp up completed"); + Serial.println(F("Ramp up completed")); right_wheel_motor_is_on = true; } } -void SETUPDEBUG::toggleCutterMotor() { - -} - -void SETUPDEBUG::updateBWF() { - sensor->readSensor(); -} void SETUPDEBUG::getBwfSignals() { - Serial.println("-------- Testing Sensors 0 -> 3 --------"); + Serial.println(F("-------- Testing Sensors 0 -> 3 --------")); for (int i=0; i<4; i++) { sensor->select(i); delay(1000); Serial.print(i); - Serial.print(": "); + Serial.print(F(": ")); sensor->printSignal(); - Serial.print(" in:"); + Serial.print(F(" in:")); Serial.print(sensor->isInside()); - Serial.print(" out:"); + Serial.print(F(" out:")); Serial.print(sensor->isOutside()); Serial.println(); } - Serial.println("Sensor test completed"); + Serial.println(F("Sensor test completed")); } -void SETUPDEBUG::getMotionValues() { +void SETUPDEBUG::toggleCutterMotor() { + if (cutter_motor_is_on) { + Serial.println(F("Ramping down cutter")); + for (int i=100; i>=0; i--) { + cutter->setSpeed(i); + delay(10); + } + Serial.println(F("Ramp down completed")); + cutterspeed = 0; + cutter_motor_is_on = false; + } + else + { + Serial.println(F("Ramping up cutter")); + for (int i=0; i<100; i++) { + cutter->setSpeed(i); + delay(10); + } + Serial.println(F("Ramp up completed")); + cutterspeed = 100; + cutter_motor_is_on = true; + } +} +void SETUPDEBUG::testRun() { + for (int i=0; i<100; i++) { + sensor->select(0); + delay(100); + rightMotor->setSpeed((sensor->isInside()?100:-100)); + + sensor->select(1); + delay(100); + leftMotor->setSpeed((sensor->isInside()?100:-100)); + } + leftMotor->setSpeed(0); + rightMotor->setSpeed(0); } -void SETUPDEBUG::toggleLed() { +void SETUPDEBUG::cutterSpeedUp() { + cutterspeed += 10; + if(cutterspeed > 100) cutterspeed = 100; + cutter->setSpeed(cutterspeed); + Serial.println(cutterspeed); +} - if (led_is_on) - digitalWrite(10,LOW); - else - digitalWrite(10,HIGH); +void SETUPDEBUG::cutterSpeedDown() { + cutterspeed -= 10; + if(cutterspeed < 0) cutterspeed = 0; + cutter->setSpeed(cutterspeed); + Serial.println(cutterspeed); +} + +void SETUPDEBUG::printStatus() { + Serial.print(F(" LMot: ")); + Serial.print(leftMotor->getLoad()); + Serial.print(F(" RMot: ")); + Serial.print(rightMotor->getLoad()); + Serial.print(F(" BAT: ")); + battery->resetVoltage(); + Serial.print(battery->getVoltage()); + Serial.print(F(" Dock: ")); + Serial.print(battery->isBeingCharged()); +} + +void SETUPDEBUG::turnRight() { + mower->turnRight(90); + mower->stop(); + Serial.print(F("If turn was not 90 degrees, consider altering TURNDELAY in definition.h")); +} + +void SETUPDEBUG::getMotionValues() { +#if __MMA7455__ + compass->autoupdate(); +#endif + + int tilt_angle = compass->getTiltAngle(); + + int x = compass->getXAngle(); + int y = compass->getYAngle(); + int z = compass->getZAngle(); + + Serial.print(F("Z = ")); + Serial.println(z); + Serial.print(F("Y = ")); + Serial.println(y); + Serial.print(F("X = ")); + Serial.println(x); - led_is_on = (led_is_on?false:true); + Serial.print(F("Tilt angle = ")); + Serial.println(tilt_angle); } diff --git a/Liam/SetupDebug.h b/Liam/SetupDebug.h index 0f22f73..c8c3ee5 100644 --- a/Liam/SetupDebug.h +++ b/Liam/SetupDebug.h @@ -23,7 +23,6 @@ class SETUPDEBUG { public: SETUPDEBUG(CONTROLLER* mower, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cut, BWFSENSOR* bwf, MOTIONSENSOR* comp, BATTERY* batt); int tryEnterSetupDebugMode(int currentState); - void updateBWF(); private: CONTROLLER* mower; WHEELMOTOR* leftMotor; @@ -32,29 +31,26 @@ class SETUPDEBUG { BWFSENSOR* sensor; MOTIONSENSOR* compass; BATTERY* battery; + void printHelp(); + void toggleLed(); void toggleWheelLeft(); void togglewheelRight(); - void toggleCutterMotor(); void getBwfSignals(); + void toggleCutterMotor(); + void testRun(); + void cutterSpeedUp(); + void cutterSpeedDown(); + void printStatus(); + void SETUPDEBUG::turnRight(); void getMotionValues(); - void toggleLed(); - int pitch; boolean led_is_on; boolean cutter_motor_is_on; boolean left_wheel_motor_is_on; boolean right_wheel_motor_is_on; - boolean cutter_is_attached; int cutterspeed; - - int16_t mx, gx, ax; - int16_t my, gy, ay; - int16_t mz, gz, az; - int x, y, z; - - int current_heading, target_heading, tilt_angle; }; #endif /* _SETUPDEBUG_H_ */ From ba4284e2e2b1b9fc7057c96d9357678b7c7346cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Fri, 1 Jun 2018 10:23:38 +0200 Subject: [PATCH 23/28] Add debug mode for adjusting trimpot --- Liam/SetupDebug.cpp | 40 ++++++++++++++++++++++++++++++++++++++++ Liam/SetupDebug.h | 1 + 2 files changed, 41 insertions(+) diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index 489a242..a8c1dc4 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -105,6 +105,10 @@ int SETUPDEBUG::tryEnterSetupDebugMode(int currentState) { case 'B': return LOOKING_FOR_BWF; + case 'a': + case 'A': + SETUPDEBUG::trimpotAdjust(); + break; } inChar = 0; @@ -123,6 +127,7 @@ void SETUPDEBUG::printHelp() { Serial.println(F("D = turn LED on/off")); Serial.println(F("T = make a 10 second test run")); Serial.println(F("P = print battery & debug values")); + Serial.println(F("A = Trimpot adjust mode")); Serial.println(F("M = Start mowing")); Serial.println(F("B = Look for BWF and dock")); } @@ -285,3 +290,38 @@ void SETUPDEBUG::getMotionValues() { Serial.print(F("Tilt angle = ")); Serial.println(tilt_angle); } + +void SETUPDEBUG::trimpotAdjust() +{ + Serial.println(F("Continuously printing battery voltage")); + Serial.println(F("Adjust your pot to match measured value")); + Serial.println(F("Send D to return to debug mode")); + Serial.print(F("Starting in 5")); + delay(1000); + Serial.print(F("...4")); + delay(1000); + Serial.print(F("...3")); + delay(1000); + Serial.print(F("...2")); + delay(1000); + Serial.println(F("...1")); + delay(1000); + while(1) + { + while(Serial.available()) + { + char inChar = (char)Serial.read(); + if (inChar == 'd' || inChar == 'D') + { + Serial.println(F("Returning to debug mode")); + return; + } + } + + battery->resetVoltage(); + Serial.print(battery->getVoltage()); + Serial.println(F(" (D to stop)")); + delay(500); + } +} + diff --git a/Liam/SetupDebug.h b/Liam/SetupDebug.h index c8c3ee5..449afc7 100644 --- a/Liam/SetupDebug.h +++ b/Liam/SetupDebug.h @@ -44,6 +44,7 @@ class SETUPDEBUG { void printStatus(); void SETUPDEBUG::turnRight(); void getMotionValues(); + void trimpotAdjust(); boolean led_is_on; From 1b4571d5de5e8ad3e1091ec2fd444c88f04c82e4 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Wed, 6 Jun 2018 08:07:26 +0200 Subject: [PATCH 24/28] Using isOUtOfBounds instead of inInside/isOutside when appropriate --- Liam/Controller.cpp | 12 ++++++------ Liam/MyDisplay.cpp | 2 +- Liam/SetupDebug.cpp | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Liam/Controller.cpp b/Liam/Controller.cpp index 382fa94..89cff08 100644 --- a/Liam/Controller.cpp +++ b/Liam/Controller.cpp @@ -29,7 +29,7 @@ boolean CONTROLLER::allSensorsAreOutside() { for(int i=0; iselect(i); - if (!sensor->isOutside()) + if (!sensor->isOutOfBounds()) return false; } @@ -52,9 +52,9 @@ int CONTROLLER::turnToReleaseLeft(int angle) { for (int i=0; i<20; i++) { sensor->select(1); - if (sensor->isInside()) { + if (!sensor->isOutOfBounds()) { sensor->select(0); - if (sensor->isInside()) + if (!sensor->isOutOfBounds()) return 0; // OK } @@ -73,9 +73,9 @@ int CONTROLLER::turnToReleaseRight(int angle) { for (int i=0; i<20; i++) { sensor->select(0); - if (sensor->isInside()) { + if (!sensor->isOutOfBounds()) { sensor->select(1); - if (sensor->isInside()) + if (!sensor->isOutOfBounds()) return 0; // OK } @@ -152,7 +152,7 @@ int CONTROLLER::GoBackwardUntilInside (BWFSENSOR *Sensor) { int counter=MAX_GO_BACKWARD_TIME; //Mover has just stoped. Let it pause for a second. delay(1000); - while(Sensor->isInside()==false) + while(!sensor->isOutOfBounds()==false) { runBackward(FULLSPEED); delay(1000); diff --git a/Liam/MyDisplay.cpp b/Liam/MyDisplay.cpp index fca3387..8dfeae4 100644 --- a/Liam/MyDisplay.cpp +++ b/Liam/MyDisplay.cpp @@ -59,7 +59,7 @@ void MYDISPLAY::update() #endif print(F("InOut: ")); - print(sensor->isInside()); + print(!sensor->isOutOfBounds()); print("\n"); // Row 2: Motor load diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index a8c1dc4..bce4c6f 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -227,11 +227,11 @@ void SETUPDEBUG::testRun() { for (int i=0; i<100; i++) { sensor->select(0); delay(100); - rightMotor->setSpeed((sensor->isInside()?100:-100)); + rightMotor->setSpeed((!sensor->isOutOfBounds()?100:-100)); sensor->select(1); delay(100); - leftMotor->setSpeed((sensor->isInside()?100:-100)); + leftMotor->setSpeed((!sensor->isOutOfBounds()?100:-100)); } leftMotor->setSpeed(0); rightMotor->setSpeed(0); From facc0fba9d73883af09c604251118b23ae47d744 Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Wed, 6 Jun 2018 08:08:01 +0200 Subject: [PATCH 25/28] print => println --- Liam/SetupDebug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Liam/SetupDebug.cpp b/Liam/SetupDebug.cpp index bce4c6f..36ab55d 100644 --- a/Liam/SetupDebug.cpp +++ b/Liam/SetupDebug.cpp @@ -266,7 +266,7 @@ void SETUPDEBUG::printStatus() { void SETUPDEBUG::turnRight() { mower->turnRight(90); mower->stop(); - Serial.print(F("If turn was not 90 degrees, consider altering TURNDELAY in definition.h")); + Serial.println(F("If turn was not 90 degrees, consider altering TURNDELAY in definition.h")); } void SETUPDEBUG::getMotionValues() { From 1873a7c64bcfd2410a33a29ed2232136cbb20536 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= Date: Wed, 6 Jun 2018 10:16:10 +0200 Subject: [PATCH 26/28] Fix check for BWF before launching --- Liam/Liam.ino | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 0433d39..48d6af5 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -385,12 +385,6 @@ void loop() //----------------------- LAUNCHING --------------------------- case LAUNCHING: - - //Don't launch without signal - if (!Sensor.isInside() && Sensor.isOutside()) { - break; - } - Mower.runBackward(FULLSPEED); delay(7000); @@ -525,12 +519,16 @@ void loop() // Just remain in this state until battery is full #if defined __RTC_CLOCK__ - if (Battery.isFullyCharged() && Clock.timeToCut()) - state = LAUNCHING; + if (Battery.isFullyCharged() && Clock.timeToCut()) { #else - if (Battery.isFullyCharged()) - state = LAUNCHING; + if (Battery.isFullyCharged()) { #endif + //Don't launch without signal + if (Sensor.isInside() || Sensor.isOutside()) + { + state = LAUNCHING; + } + } in_contact = false; From c664ce8bb721bb5544812d72cae636b3a2c7ee74 Mon Sep 17 00:00:00 2001 From: Roberth Andersson Date: Wed, 6 Jun 2018 11:39:30 +0200 Subject: [PATCH 27/28] Resolved #39 - Update voltage during CHARGING --- Liam/Liam.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/Liam/Liam.ino b/Liam/Liam.ino index 48d6af5..c6b9830 100644 --- a/Liam/Liam.ino +++ b/Liam/Liam.ino @@ -548,6 +548,7 @@ void loop() Mower.stop(); } + Battery.updateVoltage(); Serial.print("BAT:"); Serial.println(Battery.getVoltage()); From 3b0c41030e8f4a0efb4a5b3d7b889d5c3509334a Mon Sep 17 00:00:00 2001 From: Datorsmurf Date: Wed, 6 Jun 2018 20:36:32 +0200 Subject: [PATCH 28/28] BWF_DETECTION_ALWAYS should be true in release/RC --- Liam/Definition.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Liam/Definition.h b/Liam/Definition.h index 07d7a39..6b310e6 100644 --- a/Liam/Definition.h +++ b/Liam/Definition.h @@ -87,7 +87,7 @@ const int SETUP_DEBUG = 5; #pragma region BWF // BWF Detection method (true = always, false = only at wire) -#define BWF_DETECTION_ALWAYS false +#define BWF_DETECTION_ALWAYS true #define TIMEOUT 6000 //Time without signal before error #define BWF_COLLECT_SIGNAL_TIME 200 // max time to spend looking for signal #define BWF_NUMBER_OF_PULSES 3