diff --git a/Liam/Definition.cpp b/Liam/Definition.cpp
index 8b3d742..5235103 100644
--- a/Liam/Definition.cpp
+++ b/Liam/Definition.cpp
@@ -1,31 +1,47 @@
-/// This is the library for the defaults
-//
-// Changelog:
-// 2014-12-12 - Initial version by Jonas
+/*
+ Definition.h is part of the project: liam - DIY Robot Lawn mover
-/* ============================================
-Placed under the GNU license
+ Description: This file holds all configuration parameters, user specific,
+ common and software specific parameters.
+
+ 2017-06-06 Roberth Andersson:
+ - Restructured the file and made differens sections of parameters. Added setup debug mode.
+ 2014-12-12 Jonas Forsell:
+ - Initial version.
+
+ Copyright (c) 2017 Jonas Forsell (and team)
+
+ liam - DIY Robot LAwn mower is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Foobar is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Foobar. If not, see .
-===============================================
*/
#include "Definition.h"
-/** Define all pins
- */
+/* Define all pins */
void DEFINITION::definePinsInputOutput() {
-
+
pinMode(WHEEL_MOTOR_A_CURRENT_PIN, INPUT);
pinMode(WHEEL_MOTOR_B_CURRENT_PIN, INPUT);
pinMode(SOC_PIN, INPUT);
pinMode(CUTTER_CURRENT_PIN, INPUT);
-
+
/* Some pins are better leave undefined as default
pinMode(I2C_SDA_PIN, INPUT);
pinMode(I2C_SDL_PIN, OUTPUT);
- pinMode(RX_PIN, INPUT);
+ pinMode(RX_PIN, INPUT);
pinMode(TX_PIN, OUTPUT);
*/
pinMode(BWF_SENSOR_INPUT_PIN, INPUT);
@@ -34,23 +50,23 @@ void DEFINITION::definePinsInputOutput() {
pinMode(DOCK_PIN, INPUT);
pinMode(CUTTER_PWM_PIN, OUTPUT);
pinMode(BWF_SELECT_B_PIN, OUTPUT);
-
-#if defined __Bumper__
+
+#if __Bumper__
pinMode(BUMPER, INPUT);
digitalWrite(BUMPER, HIGH);
#else
pinMode(BUMPER, OUTPUT);
digitalWrite(BUMPER, LOW);
-#endif
+#endif
-#if defined __Lift_Sensor__
+#if __Lift_Sensor__
pinMode(LIFT_SENSOR_PIN, INPUT);
digitalWrite(LIFT_SENSOR_PIN, HIGH);
#else
pinMode(LIFT_SENSOR_PIN, OUTPUT);
digitalWrite(LIFT_SENSOR_PIN, LOW);
-#endif
-
+#endif
+
pinMode(LED_PIN, OUTPUT);
pinMode(WHEEL_MOTOR_B_PWM_PIN, OUTPUT);
pinMode(WHEEL_MOTOR_A_DIRECTION_PIN, OUTPUT);
diff --git a/Liam/Definition.h b/Liam/Definition.h
index 959d0ee..a94133a 100644
--- a/Liam/Definition.h
+++ b/Liam/Definition.h
@@ -1,11 +1,33 @@
-// This is the definition for all the defaults
-//
+/*
+ Definition.h is part of the project: liam - DIY Robot Lawn mover
-/* ============================================
-Placed under the GNU license
+ Description: This file holds all configuration parameters, user specific,
+ common and software specific parameters.
+
+ 2017-06-06 Roberth Andersson:
+ - Restructured the file and made differens sections of parameters.
+ - Added setup debug mode.
+ 2014-12-12 Jonas Forsell:
+ - Initial version.
+
+ Copyright (c) 2014-2017 Jonas Forsell (and team)
+
+ liam - DIY Robot LAwn mower is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Foobar is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Foobar. If not, see .
-===============================================
*/
+
+
#include
#include "Wheelmotor.h"
#include "CutterMotor.h"
@@ -14,130 +36,167 @@ Placed under the GNU license
#ifndef _DEFINITION_H_
#define _DEFINITION_H_
-const int NUMBER_OF_SENSORS = 3; // Number of BWF sensors can be 1-4 depending on shield
-
-// 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 CUTTER_CURRENT_PIN 3
-#define I2C_SDA_PIN 4
-#define I2C_SDL_PIN 5
-// Digital pins
-#define RX_PIN 0
-#define TX_PIN 1
-#define BWF_SENSOR_INPUT_PIN 2
-#define WHEEL_MOTOR_A_PWM_PIN 3
-#define BWF_SELECT_A_PIN 4
-#define DOCK_PIN 5
-#define CUTTER_PWM_PIN 6
-#define BWF_SELECT_B_PIN 7
-#define BUMPER 8
-#define LIFT_SENSOR_PIN 9
-#define LED_PIN 13
-#define WHEEL_MOTOR_B_PWM_PIN 11
-#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 LEADACID 0
-#define NIMH 1
-#define LIION 2
-
-// Wheel motor
-#define WHEELMOTOR_OVERLOAD 130
-#define WHEELMOTOR_SMOOTHNESS 300
-
-// If the mower has repeated overload within the interval below (in milliseconds), it will flag as having reached a bump.
-// It will then do some action as stated in the Controller.cpp file.
-#define OVERLOAD_INTERVAL 3000
+/* SETUP AND DEBUG MODE .
+ First time you start your mover the code is configured to run in debug mode.
+ In that mode you can send different commands to the mover to test different functions.
+ You can also see some values reported by the sensors. You need probably to tweek some
+ of the parameters in this file and when you are done remove or comment out this line to
+ run your mover in real mode. */
+#define __SETUP_AND_DEBUG_MODE__ true
+
+/******************************************************************
+ User specific settings depends on how you have built your mover.
+*******************************************************************/
+
+
+/* Configure which Cutter motor you have.
+ Types available:
+ BRUSHLESS 0 (for all hobbyking motors with external ESC)
+ BRUSHED: 1 (for all brushed motors, A3620 and NIDEC 22)
+ NIDEC 2 (for NIDEC 24 or NIDEC 22 connected to morgan shield without any modifications) */
+const int MY_CUTTERMOTOR = 2;
+
+/* Configure which type of battery you have.
+ Types availible:
+ LEADACID 0
+ NIMH 1
+ LIION 2
+*/
+const int MY_BATTERY = 2;
-// CUTTER
-#define CUTTER_OVERLOAD 100
+/* Number of BWF sensors can be 1-4 depending on shield */
+const int NUMBER_OF_SENSORS = 2;
-// Cutter states
-const int MOWING = 0;
-const int LAUNCHING = 1;
-const int DOCKING = 2;
-const int CHARGING = 3;
+/* BWF transmitter signals */
+#define INSIDE_BWF 5
+#define OUTSIDE_BWF 86
-// Turning details
-#define TURN_INTERVAL 15000
-#define REVERSE_DELAY 2
-#define TURNDELAY 20 //Reduce for smaller turning angle
+/* If you have a bumper connected to pin8, set it to true. Remember to cut the brake connection on your motor shield */
+#define __Bumper__ false
-// BWF Detection method (true = always, false = only at wire)
-#define BWF_DETECTION_ALWAYS true
-#define TIMEOUT 6000 //Time without signal before error
+/* If you have a lift sensor connection to front wheel (connected to pin9), set it to true */
+#define __Lift_Sensor__ false
-// 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
+/* Do you have a Sensor? If so, set one of these lines to true. */
+#define __MS5883L__ false
+#define __MS9150__ false
+#define __ADXL345__ true
-// Code for inside and outside
-//#define INSIDE_BWF 103,4,103
-//#define OUTSIDE_BWF 103,107,103
+/* Tiltangle */
+#define TILTANGLE 45
-// Version 2 of the BWF transmitter
-#define INSIDE_BWF 5
-#define OUTSIDE_BWF 86
+/* Flipangle (turn off cutter and stop everything) */
+#define FLIPANGLE 75
-#define MAJOR_VERSION 4
-#define MINOR_VERSION_1 9
-#define MINOR_VERSION_2 1
+/* Do you have a Display? If so, set one of these lines to true. */
+#define __OLED__ false
+#define __LCD__ false
-// If you have a bumper connected to pin8, uncomment the line below. Remember to cut the brake connection on your motor shield
-//#define __Bumper__
+/* Do you have a clock? If so, set it to true. */
+#define __RTC_CLOCK__ false
+#define GO_OUT_TIME 16, 00
+#define GO_HOME_TIME 22, 00
-// If you have a lift sensor connection to front wheel (connected to pin9), uncomment this line
-//#define __Lift_Sensor__
+/* Motor Speeds */
+#define FULLSPEED 100
+#define SLOWSPEED 30
+#define CUTTERSPEED 100
-// Do you have a Sensor? If so, uncomment one of these lines
-//#define __MS5883L__
-//#define __MS9150__
-//#define __ADXL345__
+/* Settings for ADXL345, what angle values the sensor reports when the mover is standing flat.
+ IMPORTANT! You must calibrate those values for your setup.
+ See the wiki:https://github.com/sm6yvr/liam/wiki/12.-Gyro-Accelerometer */
+ /* Try to get inside for max x seconds, then stop and error. */
+#define Z_ANGLE_NORMAL 181
+#define Y_ANGLE_NORMAL 195
+
+/* Enable this if you need the mower to go backward until it's inside and then turn.
+ Default behavior is to turn directly when mower is outside BWF, if definition below is enabled this might help mower not to get stuck in slopes.
+ If mower is not inside within x seconds mower will stop. */
+#define GO_BACKWARD_UNTIL_INSIDE false
+#define MAX_GO_BACKWARD_TIME 5
+
+/******************************************************************
+ Common settings that should be same for the most of us.
+*******************************************************************/
+/* Pin setup following Morgan 1.5 shield and up */
+/* Analog pins */
+#define WHEEL_MOTOR_A_CURRENT_PIN 0
+#define WHEEL_MOTOR_B_CURRENT_PIN 1
+#define SOC_PIN 2
+#define CUTTER_CURRENT_PIN 3
+#define I2C_SDA_PIN 4
+#define I2C_SDL_PIN 5
+/* Digital pins */
+#define RX_PIN 0
+#define TX_PIN 1
+#define BWF_SENSOR_INPUT_PIN 2
+#define WHEEL_MOTOR_A_PWM_PIN 3
+#define BWF_SELECT_A_PIN 4
+#define DOCK_PIN 5
+#define CUTTER_PWM_PIN 6
+#define BWF_SELECT_B_PIN 7
+#define BUMPER 8
+#define LIFT_SENSOR_PIN 9
+#define LED_PIN 13
+#define WHEEL_MOTOR_B_PWM_PIN 11
+#define WHEEL_MOTOR_A_DIRECTION_PIN 12
+#define WHEEL_MOTOR_B_DIRECTION_PIN 13
+
+/******************************************************************
+ Program settings that shold not be changed.
+*******************************************************************/
+
+/* Cutter motor types */
+#define BRUSHLESS 0
+#define BRUSHED 1
+#define NIDEC 2
+
+/* Battery */
+#define LEADACID 0
+#define NIMH 1
+#define LIION 2
+
+/* Wheel motor */
+#define WHEELMOTOR_OVERLOAD 130
+#define WHEELMOTOR_SMOOTHNESS 300
-// Do you have a Display? If so, uncomment one of these lines
-//#define __OLED__
-//#define __LCD__
+/* If the mower has repeated overload within the interval below (in milliseconds),
+ it will flag as having reached a bump. It will then do some action as stated
+ in the Controller.cpp file. */
+#define OVERLOAD_INTERVAL 3000
-// Do you have a clock? If so, uncomment the line below
-//#define __RTC_CLOCK__
-#define GO_OUT_TIME 16, 00
-#define GO_HOME_TIME 22, 00
+/* CUTTER */
+#define CUTTER_OVERLOAD 100
-// Tiltangle
-#define TILTANGLE 45
+/* Cutter states */
+const int MOWING = 0;
+const int LAUNCHING = 1;
+const int DOCKING = 2;
+const int CHARGING = 3;
-// Flipangle (turn off cutter and stop everything)
-#define FLIPANGLE 75
+/* Turning details */
+#define TURN_INTERVAL 15000
+#define REVERSE_DELAY 2
+#define TURNDELAY 20 //Reduce for smaller turning angle
-// Motor Speeds
-#define FULLSPEED 100
-#define SLOWSPEED 30
-#define CUTTERSPEED 100
+/* BWF Detection method (true = always, false = only at wire) */
+#define BWF_DETECTION_ALWAYS true
+/* Time without signal before error */
+#define TIMEOUT 6000
-/* Settings for ADXL345, what angle values the sensor reports when the mover is standing flat.
-* IMPORTANT! You must calibrate those values for your setup, see the wiki:https://github.com/sm6yvr/liam/wiki/12.-Gyro-Accelerometer */
-#define Z_ANGLE_NORMAL 195
-#define Y_ANGLE_NORMAL 180
+/* 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
-// Enable this if you need the mower to go backward until it's inside and then turn.
-// Default behavior is to turn directly when mower is outside BWF, if definition below is enabled this might help mower not to get stuck in slopes.
-// If mower is not inside within x seconds mower will stop.
-//#define GO_BACKWARD_UNTIL_INSIDE
-//#define MAX_GO_BACKWARD_TIME 5 // try to get inside for max x seconds, then stop and error.
+/* Software version */
+#define MAJOR_VERSION 5
+#define MINOR_VERSION_1 2
+#define MINOR_VERSION_2 0
class DEFINITION {
public:
void definePinsInputOutput();
void setDefaultLevels(BATTERY* battery, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERMOTOR* cutter);
-
private:
};
diff --git a/Liam/Liam.ino b/Liam/Liam.ino
index e7548e4..3dcbb73 100644
--- a/Liam/Liam.ino
+++ b/Liam/Liam.ino
@@ -22,15 +22,22 @@
(c) Jonas Forssell & team
Free to use for all.
- Changelog:
-
- 5.2-SNAPSHOT
-
- - 2017-06-07 Roberth Andersson: Added gyro sensor ADXL345 support
- - 2017-06-05 Jonas Forsell: Added function Obstacle when docking
- - 2017-06-02 Ola Palm: Added function GO_BACKWARD_UNTIL_INSIDE
-
- 5.1
+ Changelog:
+
+ 5.2-SNAPSHOT
+ - 2017-06-08 Roberth Andersson :
+ - New structure in definition.h so the user-specific parameters
+ are in the beginning.
+ - Changed how to enable/disable the different parts in the code.
+ - Moved user-config from Liam.ino to definition.h
+ - Merged Liam5_Setup_and_Debug into Liam and added a new SetupDebug mode.
+ - 2017-06-07 Roberth Andersson:
+ - Added gyro sensor ADXL345 support
+ - 2017-06-05 Jonas Forsell:
+ - Added function Obstacle when docking
+ - 2017-06-02 Ola Palm:
+ - Added function GO_BACKWARD_UNTIL_INSIDE
+ 5.1
- Removed OzOLED Support for Arduino101 Compatibility
- -----------------------------------------------------------
@@ -74,29 +81,24 @@ int LCDi = 0;
// Set up all the defaults (check the Definition.h file for all default values)
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(MY_CUTTERMOTOR, 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, SOC_PIN, DOCK_PIN);
+BATTERY Battery(MY_BATTERY, SOC_PIN, DOCK_PIN);
// BWF Sensors
BWFSENSOR Sensor(BWF_SELECT_B_PIN, BWF_SELECT_A_PIN);
// Compass
-#if defined __MS5883L__
+#if __MS5883L__
MS5883L Compass;
-#elif defined __MS9150__
+#elif __MS9150__
MS9150 Compass;
-#elif defined __ADXL345__
+#elif __ADXL345__
SENSADXL345 Compass;
#else
MOTIONSENSOR Compass;
@@ -106,20 +108,38 @@ BWFSENSOR Sensor(BWF_SELECT_B_PIN, BWF_SELECT_A_PIN);
CONTROLLER Mower(&leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass);
// Display
-#if defined __LCD__
+#if __LCD__
myLCD Display(&Battery, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &state);
#else
MYDISPLAY Display(&Battery, &leftMotor, &rightMotor, &CutterMotor, &Sensor, &Compass, &state);
#endif
// RTC klocka
-#if defined __RTC_CLOCK__
+#if __RTC_CLOCK__
CLOCK myClock;
#endif
// Error handler
ERROR Error(&Display, LED_PIN, &Mower);
+#if __SETUP_AND_DEBUG_MODE__
+ int pitch = 0;
+
+ boolean cutter_motor_is_on;
+ boolean left_wheel_motor_is_on;
+ boolean right_wheel_motor_is_on;
+ boolean led_is_on = false;
+ boolean cutter_is_attached = false;
+
+ int cutterspeed = 0;
+
+ int16_t mx, gx, ax;
+ int16_t my, gy, ay;
+ int16_t mz, gz, az;
+
+ int current_heading, target_heading;
+#endif
+
// This function calls the sensor object every time there is a new signal pulse on pin2
void updateBWF() {
Sensor.readSensor();
@@ -141,7 +161,7 @@ void setup()
Battery.resetSOC(); // Set the SOC to current value
Compass.initialize();
- #if defined __RTC_CLOCK__
+ #if __RTC_CLOCK__
myClock.initialize();
myClock.setGoOutTime(GO_OUT_TIME);
myClock.setGoHomeTime(GO_HOME_TIME);
@@ -150,14 +170,19 @@ void setup()
attachInterrupt(0, updateBWF, RISING); // Run the updateBWF function every time there is a pulse on digital pin2
Sensor.select(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 __SETUP_AND_DEBUG_MODE__
+ Serial.println("Welcome to Liam Test Program");
+ Serial.println("Send 'H' for list of commands");
+ #else
+ 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);
+ }
+ #endif
}
@@ -166,324 +191,542 @@ void setup()
// ***************** Main loop ***********************************
void loop()
{
- boolean in_contact;
- boolean mower_is_outside;
- int err=0;
- LCDi++; //Loops 0-10
- if (LCDi % 25 == 0 ) {
- Display.update();
- }
-
- // Security check Mower is flipped/lifted.
- #if defined __MS9150__ || defined __MS5883L__ || defined __ADXL345__
- if (Mower.hasFlipped()) {
- Serial.print("Mower has flipped ");
- Mower.stopCutter();
- Mower.stop();
- Error.flag(9);
- }
- #endif
-
- #if defined __Lift_Sensor__
- if (Mower.isLifted())
- {
- Serial.println("Mower is lifted");
- Mower.stopCutter();
- Mower.stop();
- delay(500);
- Mower.runBackward(FULLSPEED);
- delay(2000);
- if(Mower.isLifted())
- Error.flag(4);
- Mower.turnRight(90);
- //Mover.startCutter();
- Mower.runForward(FULLSPEED);
- }
- #endif
+ #if __SETUP_AND_DEBUG_MODE__
+ char inChar;
+ while (!Serial.available()); // Stay here until data is available
+ inChar = (char)Serial.read(); // get the new byte:
+
+ switch (inChar) {
+ case 'H':
+ case 'h':
+ 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 SOC & debug values");
+ Serial.println("E = Cutter motor calibrate");
+ break;
+
+ case 'D':
+ case 'd':
+ if (led_is_on)
+ digitalWrite(10,LOW);
+ else
+ digitalWrite(10,HIGH);
+
+ led_is_on = (led_is_on?false:true);
+ break;
- // Check if stuck and trigger action
- Mower.updateBalance();
-
- if (abs(Mower.getBalance()) > BALANCE_TRIGGER_LEVEL) {
- Mower.storeState();
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.stop();
- Mower.restoreState();
- Mower.resetBalance();
- }
-
- switch (state) {
-
- //------------------------- MOWING ---------------------------
- case MOWING:
- Battery.updateSOC();
- Display.update();
-
- Sensor.select(0);
-
- if (BWF_DETECTION_ALWAYS)
- mower_is_outside = !Sensor.isInside();
- else
- mower_is_outside = Sensor.isOutside();
-
- // Check left sensor (0) and turn right if needed
- if (mower_is_outside) {
- Serial.println("Left outside");
- Serial.println(Battery.getSOC());
- Mower.stop();
- #ifdef GO_BACKWARD_UNTIL_INSIDE
- err=Mower.GoBackwardUntilInside (&Sensor);
- 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) {
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.stop();
- if (err = Mower.turnToReleaseRight(30) > 0)
- Error.flag(err);
- }
-
- Compass.setNewTargetHeading();
-
- if (Mower.allSensorsAreOutside()) {
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.stop();
- if (Mower.allSensorsAreOutside())
- Error.flag(4);
- }
- }
-
- Sensor.select(1);
-
- if (BWF_DETECTION_ALWAYS)
- mower_is_outside = !Sensor.isInside();
- else
- mower_is_outside = Sensor.isOutside();
-
- // Check right sensor (1) and turn left if needed
- if (mower_is_outside) {
- Serial.println("Right Outside");
- Serial.println(Battery.getSOC());
- Mower.stop();
-
- #ifdef GO_BACKWARD_UNTIL_INSIDE
- err=Mower.GoBackwardUntilInside(&Sensor);
- if(err)
- Error.flag(err);
- #endif
-
- // Tries to turn, but if timeout then reverse and try again
- if (err = Mower.turnToReleaseLeft(30) > 0) {
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.stop();
- if (err = Mower.turnToReleaseLeft(30) > 0)
- Error.flag(err);
- }
-
- Compass.setNewTargetHeading();
-
- if (Mower.allSensorsAreOutside()) {
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.stop();
- if (Mower.allSensorsAreOutside())
- Error.flag(4);
- }
- }
-
-
- Mower.runForward(FULLSPEED);
-
- // Adjust the speed of the mower to the grass thickness
- Mower.compensateSpeedToCutterLoad();
-
- // Adjust the speed of the mower to the compass heading
- 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);
- }
-
- // 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())
- {
- Serial.println("Mower is lifted");
- Mower.stopCutter();
- Mower.runBackward(FULLSPEED);
- delay(2000);
- if(Mower.isLifted())
- Error.flag(4);
- Mower.turnRight(90);
- Mower.startCutter();
- Mower.runForward(FULLSPEED);
- }
- #endif
-
- // Check if mower has tilted (providing you have one enabled)
- #if defined __MS9150__ || defined __MS5883L__ || defined __ADXL345__
- if (Mower.hasFlipped()) {
- Serial.print("Mower has flipped ");
- Mower.stopCutter();
- Mower.stop();
- Error.flag(9);
- } else if (Mower.hasTilted()) {
- Serial.print("Mower has tilted ");
- Mower.runBackward(FULLSPEED);
- delay(2000);
- Mower.turnRight(90);
- Mower.runForward(FULLSPEED);
- delay(200);
+ case 'L':
+ case 'l':
+ if (left_wheel_motor_is_on == true) {
+ Serial.println("Ramping down left wheel");
+ for (int i=100; i>0; i--) {
+ leftMotor.setSpeed(i);
+ delay(10);
+ }
+ Serial.println("Ramp down completed");
+ left_wheel_motor_is_on = false;
+ break;
}
- #endif
-
- break;
-
-
- //----------------------- LAUNCHING ---------------------------
- case LAUNCHING:
-
- Mower.runBackward(FULLSPEED);
-
- delay(7000);
- Mower.stop();
-
- // Turn right in random degree
- Mower.turnRight(random(30,60));
- Mower.startCutter();
- Mower.waitWhileChecking(5000);
-
- Compass.setNewTargetHeading();
-
- Mower.runForward(FULLSPEED);
-
- state = MOWING;
-
- // Reset the running average
- Battery.resetSOC();
-
- break;
-
- //----------------------- DOCKING -----------------------------
- case DOCKING:
+ else
+ {
+ Serial.println("Ramping up left wheel");
+ for (int i=0; i<100; i++) {
+ leftMotor.setSpeed(i);
+ delay(10);
+ }
+ Serial.println("Ramp up completed");
+ left_wheel_motor_is_on = true;
+ break;
+ }
+ break;
+
+ case 'R':
+ case 'r':
+ if (right_wheel_motor_is_on == true) {
+ Serial.println("Ramping down right wheel");
+ for (int i=100; i>0; i--) {
+ rightMotor.setSpeed(i);
+ delay(10);
+ }
+ Serial.println("Ramp down completed");
+ right_wheel_motor_is_on = false;
+ break;
+ }
+ else
+ {
+ Serial.println("Ramping up right wheel");
+ for (int i=0; i<100; i++) {
+ rightMotor.setSpeed(i);
+ delay(10);
+ }
+ Serial.println("Ramp up completed");
+ right_wheel_motor_is_on = true;
+ break;
+ }
+ break;
+
+ case 'S':
+ case 's':
+ Serial.println("-------- Testing Sensors 0 -> 3 --------");
+ attachInterrupt(0,updateBWF, RISING);
+ for (int i=0; i<4; i++) {
+ Sensor.select(i);
+ delay(1000);
+ Serial.print(i);
+ Serial.print(": ");
+ Sensor.printSignal();
+ Serial.print(" in:");
+ Serial.print(Sensor.isInside());
+ Serial.print(" out:");
+ Serial.print(Sensor.isOutside());
+ Serial.println();
+ }
+ Serial.println("Sensor test completed");
+ detachInterrupt(0);
+ break;
+
+
+ case 'C':
+ case 'c':
+ if (cutter_motor_is_on) {
+ Serial.println("Ramping down cutter");
+ for (int i=100; i>=0; i--) {
+ CutterMotor.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++) {
+ CutterMotor.setSpeed(i);
+ delay(10);
+ }
+ Serial.println("Ramp up completed");
+ cutter_motor_is_on = true;
+ }
+ break;
- //Make the wheel motors extra responsive
- leftMotor.setSmoothness(10);
- rightMotor.setSmoothness(10);
+ case 'T':
+ case 't':
+ attachInterrupt(0,updateBWF, RISING);
- // If the mower hits something, reverse and try again
- if (Mower.wheelsAreOverloaded()){
- Mower.runBackward(FULLSPEED);
- delay(1000);
- }
+ for (int i=0; i<100; i++) {
+ Sensor.select(0);
+ delay(100);
+ rightMotor.setSpeed((Sensor.isInside()?100:-100));
- // See if mower has repeated overload
- // If so, turn away from the BWF and try to hook on somewhere else
- if (Mower.hasReachedAStop()) {
- Mower.runBackward(FULLSPEED);
- delay(1000);
- Mower.turnRight(90);
- Mower.runForward(FULLSPEED);
- delay(1000);
- Mower.startCutter();
- state = MOWING;
- break;
- }
+ Sensor.select(1);
+ delay(100);
+ leftMotor.setSpeed((Sensor.isInside()?100:-100));
+ }
+ leftMotor.setSpeed(0);
+ rightMotor.setSpeed(0);
+
+ break;
+
+ case '+':
+ cutterspeed += 10;
+ CutterMotor.setSpeed(cutterspeed);
+ Serial.println(cutterspeed);
+ break;
+
+ case '-':
+ cutterspeed -= 10;
+ CutterMotor.setSpeed(cutterspeed);
+ Serial.println(cutterspeed);
+ break;
+
+ case 'P':
+ case 'p':
+ Serial.print(" LMot: ");
+ Serial.print(leftMotor.getLoad());
+ Serial.print(" RMot: ");
+ Serial.print(rightMotor.getLoad());
+ Serial.print(" SOC: ");
+ Battery.resetSOC();
+ Serial.print(Battery.getSOC());
+ Serial.print(" Dock: ");
+ Serial.print(Battery.isBeingCharged());
+ break;
+
+ // case 'e':
+ // case 'E':
+ // if (cutter_is_attached) {
+ // CutterMotor.detachMotor();
+ // Serial.println("Cutter is detached");
+ // }
+ // else {
+ // CutterMotor.initialize();
+ // Serial.println("Cutter is attached");
+ // }
+ // cutter_is_attached = !cutter_is_attached;
+ // break;
+
+ case 'g':
+ case 'G':
+ int tilt_angle = Compass.getTiltAngle();
+ int y = Compass.getYAngle();
+ int z = Compass.getZAngle();
+
+ Serial.print("RAW Z = ");
+ Serial.println(z);
+ Serial.print("RAW Y = ");
+ Serial.println(y);
+ 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;
+ break;
+
+
+ }
+
+ inChar = 0;
+ #else
+ /* MAIN PROGRAM */
+ boolean in_contact;
+ boolean mower_is_outside;
+ int err=0;
+ LCDi++; //Loops 0-10
+ if (LCDi % 25 == 0 ) {
+ Display.update();
+ }
+
+ // Security check Mower is flipped/lifted.
+ #if __MS9150__ || __MS5883L__ || __ADXL345__
+ if (Mower.hasFlipped()) {
+ Serial.print("Mower has flipped ");
+ Mower.stopCutter();
+ Mower.stop();
+ Error.flag(9);
+ }
+ #endif
+
+ #if __Lift_Sensor__
+ if (Mower.isLifted())
+ {
+ Serial.println("Mower is lifted");
+ Mower.stopCutter();
+ Mower.stop();
+ delay(500);
+ Mower.runBackward(FULLSPEED);
+ delay(2000);
+ if(Mower.isLifted())
+ Error.flag(4);
+ Mower.turnRight(90);
+ //Mover.startCutter();
+ Mower.runForward(FULLSPEED);
+ }
+ #endif
+
+ // Check if stuck and trigger action
+ Mower.updateBalance();
+
+ if (abs(Mower.getBalance()) > BALANCE_TRIGGER_LEVEL) {
+ Mower.storeState();
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.stop();
+ Mower.restoreState();
+ Mower.resetBalance();
+ }
+
+ switch (state) {
+
+ //------------------------- MOWING ---------------------------
+ case MOWING:
+ Battery.updateSOC();
+ Display.update();
+
+ Sensor.select(0);
+
+ if (BWF_DETECTION_ALWAYS)
+ mower_is_outside = !Sensor.isInside();
+ else
+ mower_is_outside = Sensor.isOutside();
+
+ // Check left sensor (0) and turn right if needed
+ if (mower_is_outside) {
+ Serial.println("Left outside");
+ Serial.println(Battery.getSOC());
+ Mower.stop();
+ #ifdef GO_BACKWARD_UNTIL_INSIDE
+ err=Mower.GoBackwardUntilInside (&Sensor);
+ 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) {
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.stop();
+ if (err = Mower.turnToReleaseRight(30) > 0)
+ Error.flag(err);
+ }
+
+ Compass.setNewTargetHeading();
+
+ if (Mower.allSensorsAreOutside()) {
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.stop();
+ if (Mower.allSensorsAreOutside())
+ Error.flag(4);
+ }
+ }
+
+ Sensor.select(1);
+
+ if (BWF_DETECTION_ALWAYS)
+ mower_is_outside = !Sensor.isInside();
+ else
+ mower_is_outside = Sensor.isOutside();
+
+ // Check right sensor (1) and turn left if needed
+ if (mower_is_outside) {
+ Serial.println("Right Outside");
+ Serial.println(Battery.getSOC());
+ Mower.stop();
+
+ #ifdef GO_BACKWARD_UNTIL_INSIDE
+ err=Mower.GoBackwardUntilInside(&Sensor);
+ if(err)
+ Error.flag(err);
+ #endif
+
+ // Tries to turn, but if timeout then reverse and try again
+ if (err = Mower.turnToReleaseLeft(30) > 0) {
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.stop();
+ if (err = Mower.turnToReleaseLeft(30) > 0)
+ Error.flag(err);
+ }
+
+ Compass.setNewTargetHeading();
+
+ if (Mower.allSensorsAreOutside()) {
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.stop();
+ if (Mower.allSensorsAreOutside())
+ Error.flag(4);
+ }
+ }
+
+
+ Mower.runForward(FULLSPEED);
+
+ // Adjust the speed of the mower to the grass thickness
+ Mower.compensateSpeedToCutterLoad();
+
+ // Adjust the speed of the mower to the compass heading
+ 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);
+ }
+
+ // Check if bumper has triggered (providing you have one enabled)
+ #if __Bumper__
+ if (Mower.hasBumped())
+ {
+ Serial.print("Mower has bumped ");
+ Mower.runBackward(FULLSPEED);
+ delay(2000);
+ Mower.turnRight(90);
+ Mower.runForward(FULLSPEED);
+ }
+ #endif
+
+ #if __Lift_Sensor__
+ if (Mower.isLifted())
+ {
+ Serial.println("Mower is lifted");
+ Mower.stopCutter();
+ Mower.runBackward(FULLSPEED);
+ delay(2000);
+ if(Mower.isLifted())
+ Error.flag(4);
+ Mower.turnRight(90);
+ Mower.startCutter();
+ Mower.runForward(FULLSPEED);
+ }
+ #endif
+
+ // Check if mower has tilted (providing you have one enabled)
+ #if __MS9150__ || __MS5883L__ || __ADXL345__
+ if (Mower.hasFlipped()) {
+ Serial.print("Mower has flipped ");
+ Mower.stopCutter();
+ Mower.stop();
+ Error.flag(9);
+ } else if (Mower.hasTilted()) {
+ Serial.print("Mower has tilted ");
+ Mower.runBackward(FULLSPEED);
+ delay(2000);
+ Mower.turnRight(90);
+ Mower.runForward(FULLSPEED);
+ delay(200);
+ }
+ #endif
+
+ break;
+
+
+ //----------------------- LAUNCHING ---------------------------
+ case LAUNCHING:
+
+ Mower.runBackward(FULLSPEED);
+
+ delay(7000);
+ Mower.stop();
+
+ // Turn right in random degree
+ Mower.turnRight(random(30,60));
+ Mower.startCutter();
+ Mower.waitWhileChecking(5000);
+
+ Compass.setNewTargetHeading();
+
+ Mower.runForward(FULLSPEED);
+
+ state = MOWING;
+
+ // Reset the running average
+ Battery.resetSOC();
+
+ break;
+
+ //----------------------- DOCKING -----------------------------
+ case DOCKING:
+
+ //Make the wheel motors extra responsive
+ leftMotor.setSmoothness(10);
+ rightMotor.setSmoothness(10);
+
+ // If the mower hits something, reverse and try again
+ if (Mower.wheelsAreOverloaded()){
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ }
+
+ // See if mower has repeated overload
+ // If so, turn away from the BWF and try to hook on somewhere else
+ if (Mower.hasReachedAStop()) {
+ Mower.runBackward(FULLSPEED);
+ delay(1000);
+ Mower.turnRight(90);
+ Mower.runForward(FULLSPEED);
+ delay(1000);
+ Mower.startCutter();
+ state = MOWING;
+ break;
+ }
- // Track the BWF by compensating the wheel motor speeds
- Mower.adjustMotorSpeeds();
+ // Track the BWF by compensating the wheel motor speeds
+ Mower.adjustMotorSpeeds();
- // Clear signal to allow the mower to track the wire closely
- Sensor.clearSignal();
+ // Clear signal to allow the mower to track the wire closely
+ Sensor.clearSignal();
- // Wait a little to avoid current spikes
- delay(100);
+ // 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;
- }
+ // Stop the mower as soon as the charge plates come in contact
+ if (Battery.isBeingCharged()) {
+ // Stop
+ Mower.stop();
+ Mower.resetBalance();
+ state = CHARGING;
+ break;
+ }
- break;
+ break;
- //----------------------- CHARGING ----------------------------
- case CHARGING:
- // restore wheelmotor smoothness
- leftMotor.setSmoothness(WHEELMOTOR_SMOOTHNESS);
- rightMotor.setSmoothness(WHEELMOTOR_SMOOTHNESS);
+ //----------------------- CHARGING ----------------------------
+ case CHARGING:
+ // restore wheelmotor smoothness
+ leftMotor.setSmoothness(WHEELMOTOR_SMOOTHNESS);
+ rightMotor.setSmoothness(WHEELMOTOR_SMOOTHNESS);
- // Just remain in this state until battery is full
- #if defined __RTC_CLOCK__
- if (Battery.isFullyCharged() && myClock.timeToCut())
- state = LAUNCHING;
- #else
- if (Battery.isFullyCharged())
- state = LAUNCHING;
- #endif
+ // Just remain in this state until battery is full
+ #if __RTC_CLOCK__
+ if (Battery.isFullyCharged() && myClock.timeToCut())
+ state = LAUNCHING;
+ #else
+ if (Battery.isFullyCharged())
+ state = LAUNCHING;
+ #endif
- in_contact = false;
+ in_contact = false;
- // Spend 20 seconds collecting status if being charged
- for (int i=0; i<20; i++) {
- if (Battery.isBeingCharged())
- in_contact = true;
- delay(1000);
- }
+ // Spend 20 seconds collecting status if being charged
+ for (int i=0; i<20; i++) {
+ if (Battery.isBeingCharged())
+ in_contact = true;
+ delay(1000);
+ }
- // If the mower is not being charged, jiggle it a bit
- if (!in_contact) {
- Mower.runBackward(20); // Back away slow speed
- delay(500);
- Mower.runForward(20); // Dock again at slow speed
- delay(1000);
- Mower.stop();
- }
+ // If the mower is not being charged, jiggle it a bit
+ if (!in_contact) {
+ Mower.runBackward(20); // Back away slow speed
+ delay(500);
+ Mower.runForward(20); // Dock again at slow speed
+ delay(1000);
+ Mower.stop();
+ }
- Serial.print("SOC:");
- Serial.println(Battery.getSOC());
+ Serial.print("SOC:");
+ Serial.println(Battery.getSOC());
- break;
+ break;
}
+ #endif
}
diff --git a/Liam/MyDisplay.cpp b/Liam/MyDisplay.cpp
index 7c4d160..3a862b0 100644
--- a/Liam/MyDisplay.cpp
+++ b/Liam/MyDisplay.cpp
@@ -26,16 +26,16 @@ MYDISPLAY::MYDISPLAY(BATTERY* batt, WHEELMOTOR* left, WHEELMOTOR* right, CUTTERM
// Do not override
boolean MYDISPLAY::initialize() {
char buffer [9]; //Format 09.00.00
-
+
current_row = 0;
current_col = 0;
- for (int i=0; i<3; i++)
+ for (int i=0; i<3; i++)
blink();
- setCursor(0, 0);
+ setCursor(0, 0);
print("SW version:");
- setCursor(0, 1);
+ setCursor(0, 1);
sprintf (buffer, "%d.%d.%d", MAJOR_VERSION, MINOR_VERSION_1, MINOR_VERSION_2);
print(buffer);
setCursor(0, 2);
@@ -51,7 +51,7 @@ void MYDISPLAY::update() {
int sens = 0;
// Rad 1: Sensors
- #if defined __MS9150__ || defined __MS5883L__
+ #if __MS9150__ || __MS5883L__
setCursor(0,0);
print("Comp:");
setCursor(7,0);
@@ -62,7 +62,7 @@ void MYDISPLAY::update() {
setCursor(7,0);
print("Disabled");
#endif
-
+
setCursor(10,0);
print("InOut:");
setCursor(17,0);
@@ -102,7 +102,7 @@ void MYDISPLAY::update() {
case CHARGING:
print("CHARGING");
break;
- }
+ }
}
// This is the basic implementation of the print and println command