SolarMower V1.0
SolarMower use mowershield_FE
2014 Mirco Segatello
For hardware see www.elettronicain.it
Press PEN during power-on for ESC test
Press PUP during power-on for test motor
Press PDW during power-on for test sensor
*/
#include <EEPROM.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <Wire.h>
#include <SRF02.h>
#include <Configuration.h>
#define D22DIRA_pin (22)
#define D23DIRB_pin (23)
Servo ESC;
//variables
float VBat; // Battery voltage [V]
int VBatPC; // Battery voltage percentage
float VBatScale=0.054; // Battery scale for Volt converter
float IPanel; // Panel current [A]
float IPanelOffset; // Panel current offset
float IPanelScale=0.185; // Panel current scale V/A
float ICut; // Motor cut current [A]
float ICutOffset; // Motor cut current offset
float ICutScale=0.185; // Motor cut current scale V/A
int i;
int cutPower=0; // Cut Power from 0%=ESC_MIN_SIGNAL to 100%=ESC_MAX_SIGNAL
int cutPower_uSec=ESC_MIN_SIGNAL; // Cut Power from ESC_MIN_SIGNAL to ESC_MAX_SIGNAL
int oldSpeed=0; // Speed of mower (0-255)
float IPanelMax=0; // IPanel max current
unsigned long BWFL_count;
unsigned long BWFR_count;
unsigned long previousMillis = 0;
unsigned long currentMillis;
volatile int mowerStatus=0; // 0=oncharge (press pen for run)
// 1=run
// 2=stuck
// 3=search
// 4=batlow
// 5=charge and restart when full
// 6=cuterror
volatile unsigned long wheelTime=0;
// LCD does not send data back to the Arduino, only define the txPin
SoftwareSerial LCD = SoftwareSerial(0, LCD_pin);
int LCD_Page = 0; // Page to display
SRF02 US_SX(US_SX_address, SRF02_CENTIMETERS);
SRF02 US_DX(US_DX_address, SRF02_CENTIMETERS);
void setup()
{
Serial.begin(9600);
Wire.begin();
pinMode(Encoder_Pin, INPUT_PULLUP);
pinMode(PWMA_pin, OUTPUT);
pinMode(DIRA_pin, OUTPUT);
pinMode(BWFR_pin, INPUT);
pinMode(BWFL_pin, INPUT);
pinMode(Panel_pin, OUTPUT);
pinMode(ESC_pin, OUTPUT);
pinMode(SWOR_pin, INPUT_PULLUP);
pinMode(SWOL_pin, INPUT_PULLUP);
pinMode(PWMB_pin, OUTPUT);
pinMode(DIRB_pin, OUTPUT);
pinMode(LCD_pin, OUTPUT);
pinMode(D22DIRA_pin, OUTPUT);
pinMode(D23DIRB_pin, OUTPUT);
digitalWrite(D22DIRA_pin, LOW);
digitalWrite(D23DIRB_pin, LOW);
digitalWrite(DIRA_pin, LOW);
digitalWrite(DIRB_pin, LOW);
analogWrite(PWMA_pin, 0);
analogWrite(PWMB_pin, 0);
digitalWrite(Panel_pin, LOW);
// For SerialMonitor diagnostic
Serial.println("Solar Mower V1.0\n");
Serial.println("Init ESC...");
ESC.attach(ESC_pin);
cutOFF();
Serial.println("Init SerLCD...");
serLCDInit();
clearLCD();
lcdPosition(0,0);
LCD.print("Solar Mower v1.0");
lcdPosition(1,0);
LCD.print("Init...");
Serial.println("Init Sensor");
sensorInit();
Serial.print("IPanelOffset= ");
Serial.println(IPanelOffset);
Serial.print("ICutOffset= ");
Serial.println(ICutOffset);
Serial.println();
// TEST MOTOR
if (Button(Button_pin)==PUP)
{
while(1)
{
clearLCD();
lcdPosition(0,0);
LCD.print("Test motor");
lcdPosition(1,0);
LCD.print("PEN begin test ");
Serial.println("Press PEN for begin motor test");
while (Button(Button_pin)!=3) { }
testMotor();
}
}
//TEST SENSOR
if (Button(Button_pin)==PDW)
{
clearLCD();
lcdPosition(0,0);
LCD.print("Test sensor");
Serial.println("Test sensor");
delay(1000);
while(1)
{
sensorReading();
clearLCD();
lcdPosition(0,0);
LCD.print("VB=");
LCD.print(VBat);
lcdPosition(0,9);
if ((US_DX.read()>0) && (US_DX.read()<USdistance)) {
LCD.print("UsDX ");
Serial.println("UsDX");
}
if ((US_SX.read()>0) && (US_SX.read()<USdistance)) {
LCD.print("UsSX ");
Serial.println("UsSX");
}
if ( (BWFL_count>3000) && (BWFL_count<4000) ) {
LCD.print("BWFL");
Serial.println("BWFL");
}
if ( (BWFR_count>2000) && (BWFR_count<4000) ) {
LCD.print("BWFR");
Serial.println("BWFR");
}
if (digitalRead(Encoder_Pin)==0) {
LCD.print("Encoder");
Serial.println("Encoder");
}
if (digitalRead(SWOR_pin)==0) {
LCD.print("SWOR");
Serial.println("SWOR");
}
if (digitalRead(SWOL_pin)==0) {
LCD.print("SWOL");
Serial.println("SWOL");
}
if (Button(Button_pin)==1) {
LCD.print("PUP");
Serial.println("PUP");
}
if (Button(Button_pin)==2) {
LCD.print("PDW");
Serial.println("PDW");
}
if (Button(Button_pin)==3) {
LCD.print("PEN");
Serial.println("PEN");
}
lcdPosition(1,0);
LCD.print("IC=");
LCD.print(ICut);
Serial.print("IC=");
Serial.println(ICut);
lcdPosition(1,9);
LCD.print("IP=");
LCD.print(IPanel);
Serial.print("IP=");
Serial.println(IPanel);
delay(250);
}
}
//TEST ESC
if (Button(Button_pin)==PEN)
{
Serial.println("Test ESC...");
clearLCD();
lcdPosition(0,0);
LCD.print("Test ESC");
while(1)
{
sensorReading();
cutPower = map(cutPower_uSec, ESC_MIN_SIGNAL, ESC_MAX_SIGNAL, 0, 100);
lcdPosition(0,9);
LCD.print("IC=");
LCD.print(ICut);
lcdPosition(1,0);
LCD.print("cutPower=");
LCD.print(cutPower);
LCD.print("% ");
Serial.print("cutPower=");
Serial.print(cutPower);
Serial.print("% usec=");
Serial.println(cutPower_uSec);
if (Button(Button_pin)==2)
if (cutPower_uSec<ESC_MAX_SIGNAL) cutPower_uSec += 20;
if (Button(Button_pin)==1)
if (cutPower_uSec>ESC_MIN_SIGNAL) cutPower_uSec -= 20;
ESC.writeMicroseconds(cutPower_uSec);
delay(200);
}
}
attachInterrupt(0, rotate, FALLING);
}
void loop()
{
//main program
digitalWrite(Panel_pin, HIGH);
main:
//wait for PEN press
/*while(Button(Button_pin)!=3)
{
currentMillis = millis();
if(currentMillis - previousMillis > timeClock)
{
sensorReading();
LCDdebug();
}
} */
Serial.println("GO!");
mowerStatus=1;
IPanelMax=0;
LCDdebug();
cutON();
setMowerSpeed(255);
//main loop
while(1)
{
//ferma tutto
if (Button(Button_pin)==PUP || Button(Button_pin)==PDW)
{
cutOFF();
setMowerSpeed(0);
mowerStatus=0;
goto main;
}
//if not an interrupt wheels occurs within 10 seconds, the robot is blocked
if (millis()>wheelTime+10000)
{
cutOFF();
setMowerSpeed(0);
mowerStatus=2;
goto main;
}
// gestione sensori ostacolo
if (digitalRead(SWOL_pin)==LOW) obstacleAvoidSX();
if (digitalRead(SWOR_pin)==LOW) obstacleAvoidDX();
// reads the sensors every timeClock
currentMillis = millis();
if(currentMillis - previousMillis > timeClock)
{
previousMillis = currentMillis;
sensorReading();
if ((US_DX.read()>0) && (US_DX.read()<USdistance))
{
obstacleAvoidSX();
resetEncoder();
}
if ((US_SX.read()>0) && (US_SX.read()<USdistance))
{
obstacleAvoidDX();
resetEncoder();
}
if ((BWFL_count>3000) && (BWFL_count<4000))
{
obstacleAvoidSX();
resetEncoder();
}
if ((BWFR_count>2000) && (BWFR_count<4000))
{
obstacleAvoidDX();
resetEncoder();
}
//**********************************************************************
if (IPanel>IPanelMax) IPanelMax=IPanel; //memory to max current on panel
//battery lower then 10% then searching the point to charge
if (VBatPC<10)
{
mowerStatus=3;
cutOFF(); //stop cut
}
//continues to advance until it finds a light source at least 80% of the previous maximum.
if (mowerStatus==3)
{
if (IPanel>IPanelMax*0.8)
{
setMowerSpeed(0);
mowerStatus=5; //charge with restart
}
}
//**********************************************************************
if (mowerStatus==5)
{
resetEncoder();
// full charge then restart
if (VBatPC>=100)
{
mowerStatus=1;
IPanelMax=0;
cutON();
setMowerSpeed(255);
}
}
if (VBat > VBat_Level_Max)
{
//lead battery never stop charge
//disable charge only for lipo battery
//digitalWrite(Panel_pin, LOW);
}
//battery is completely discharged
if (VBat < VBat_Level_Min)
{
cutOFF();
setMowerSpeed(0);
digitalWrite(Panel_pin, HIGH);
mowerStatus=4;
goto main;
}
// ICut too high
if (ICut > ICut_Max)
{
cutOFF();
setMowerSpeed(0);
digitalWrite(Panel_pin, HIGH);
mowerStatus=6;
goto main;
}
serialDebug();
LCDdebug();
}
}
}
void rotate()
{
wheelTime=millis();
}
void resetEncoder() //reset value for block wheels
{
wheelTime=millis();
}
void obstacleAvoidSX()
{
// obstacle avoid
setMowerSpeed(-PWMSpeed);
delay(timeReverse);
setMowerRotate(-PWMSpeed); //gira a sinistra
delay(timeRotate);
setMowerSpeed(PWMSpeed);
}
void obstacleAvoidDX()
{
// obstacle avoid
setMowerSpeed(-PWMSpeed);
delay(timeReverse);
setMowerRotate(PWMSpeed);
delay(timeRotate);
setMowerSpeed(PWMSpeed);
}
void setMowerRotate(int newSpeed)
{
//rotate mower (first set speed at zero)
constrain(newSpeed, -255, 255);
for (i=oldSpeed; i>=0; i--)
{
analogWrite(PWMA_pin, i);
analogWrite(PWMB_pin, i);
delay(accelerateTime);
}
oldSpeed=0;
if (newSpeed<0)
{
digitalWrite(DIRA_pin, HIGH);
digitalWrite(D22DIRA_pin, LOW);
digitalWrite(DIRB_pin, LOW);
digitalWrite(D23DIRB_pin, LOW);
newSpeed=-newSpeed;
}
else
{
digitalWrite(DIRA_pin, LOW);
digitalWrite(D22DIRA_pin, LOW);
digitalWrite(DIRB_pin, HIGH);
digitalWrite(D23DIRB_pin, LOW);
}
for (i=0; i<=newSpeed; i++)
{
analogWrite(PWMA_pin, i);
analogWrite(PWMB_pin, i);
delay(accelerateTime);
}
oldSpeed=newSpeed;
}
void setMowerSpeed(int newSpeed)
{
//set new speed for mower (first set speed at zero)
constrain(newSpeed, -255, 255);
for (i=oldSpeed; i>=0; i--)
{
analogWrite(PWMA_pin, i);
analogWrite(PWMB_pin, i);
delay(accelerateTime);
}
oldSpeed=0;
if (newSpeed<0)
{
digitalWrite(DIRA_pin, HIGH);
digitalWrite(D22DIRA_pin, LOW);
digitalWrite(DIRB_pin, HIGH);
digitalWrite(D23DIRB_pin, LOW);
newSpeed=-newSpeed;
}
else
{
digitalWrite(DIRA_pin, LOW);
digitalWrite(D22DIRA_pin, HIGH);
digitalWrite(DIRB_pin, LOW);
digitalWrite(D23DIRB_pin, HIGH);
}
if (newSpeed > oldSpeed)
{
for (i=oldSpeed; i<=newSpeed; i++)
{
analogWrite(PWMA_pin, i);
analogWrite(PWMB_pin, i);
delay(accelerateTime);
}
}
else
{
for (i=oldSpeed; i>=newSpeed; i--)
{
analogWrite(PWMA_pin, i);
analogWrite(PWMB_pin, i);
delay(accelerateTime);
}
}
oldSpeed=newSpeed;
}
void cutON()
{
//cutter ON
for (cutPower_uSec=ESC_MIN_SIGNAL; cutPower_uSec<=ESC_MAX_SIGNAL; cutPower_uSec += 10)
{
ESC.writeMicroseconds(cutPower_uSec);
delay(50);
}
wheelTime=millis();
}
void cutOFF()
{
//cutter OFF
ESC.writeMicroseconds(ESC_MIN_SIGNAL);
}