Skip to content

Commit

Permalink
2018 Version
Browse files Browse the repository at this point in the history
  • Loading branch information
@Holoratte
Holoratte committed Apr 22, 2018
1 parent b1eadec commit cc81045
Show file tree
Hide file tree
Showing 9 changed files with 812 additions and 200 deletions.
47 changes: 26 additions & 21 deletions code/ardumower/battery.h
View file
Expand Up @@ -2,11 +2,14 @@

// check battery voltage and decide what to do
void Robot::checkBattery(){
if (millis() < nextTimeCheckBattery) return;
if (millis() < nextTimeCheckBattery) return;
nextTimeCheckBattery = millis() + 1000;
if (batMonitor){
if ((batVoltage < batSwitchOffIfBelow) && (idleTimeSec != BATTERY_SW_OFF)) {
if ((batVoltage < batSwitchOffIfBelow) && (idleTimeSec != BATTERY_SW_OFF) && (batVoltage > chgVoltage)) {
Debug.println(F("triggered batSwitchOffIfBelow"));
Debug.print(batVoltage);
Debug.print(F("<"));
Debug.println(batSwitchOffIfBelow);
addErrorCounter(ERR_BATTERY);
delay(2000); // avois corrupting EEPROM while this is also called when power is turned OFF
beep(2, true);
Expand All @@ -19,27 +22,29 @@ if (millis() < nextTimeCheckBattery) return;
else if ((batVoltage < batGoHomeIfBelow) && (stateCurr == STATE_FORWARD)
&& (perimeterUse)) { //UNTESTED please verify
Debug.println(F("triggered batGoHomeIfBelow"));
Debug.print(batVoltage);
Debug.print(F("<"));
Debug.println(batGoHomeIfBelow);
beep(2, true);
setNextState(STATE_PERI_FIND, 0);
}
}
// check if idle and robot battery can be switched off
if ( (stateCurr == STATE_OFF) || (stateCurr == STATE_ERROR) ) {
if (idleTimeSec != BATTERY_SW_OFF){ // battery already switched off?
idleTimeSec ++; // add one second idle time
if ((batSwitchOffIfIdle != 0) && (idleTimeSec > batSwitchOffIfIdle * 60)) {
Debug.println(F("triggered batSwitchOffIfIdle"));
beep(1, true);
loadSaveErrorCounters(false); // saves error counters
loadSaveRobotStats(false); // saves robot stats
idleTimeSec = BATTERY_SW_OFF; // flag to remember that battery is switched off
Debug.println(F("BATTERY switching OFF"));
setActuator(ACT_BATTERY_SW, 0); // switch off battery
}
}
} else {
resetIdleTime();
}
// check if idle and robot battery can be switched off
if ( (stateCurr == STATE_OFF) || (stateCurr == STATE_ERROR) ) {
if (idleTimeSec != BATTERY_SW_OFF){ // battery already switched off?
idleTimeSec ++; // add one second idle time
if ((batSwitchOffIfIdle != 0) && (idleTimeSec > batSwitchOffIfIdle * 60)) {
Debug.println(F("triggered batSwitchOffIfIdle"));
beep(1, true);
loadSaveErrorCounters(false); // saves error counters
loadSaveRobotStats(false); // saves robot stats
idleTimeSec = BATTERY_SW_OFF; // flag to remember that battery is switched off
Debug.println(F("BATTERY switching OFF"));
setActuator(ACT_BATTERY_SW, 0); // switch off battery
}
}
} else {
resetIdleTime();
}
}
}


49 changes: 45 additions & 4 deletions code/ardumower/drivers.cpp
View file
Expand Up @@ -178,13 +178,54 @@ void setL9958(int pinDir, int pinPWM, int speed){
// PWM L Forward
// nPWM H Reverse
void setMC33926(int pinDir, int pinPWM, int speed){
#ifdef __AVR__
if (speed < 0){
switch (pinDir) {
case 33:
bitWrite(PORTC, 4, 1); //PC4
PinMan.analogWrite(pinPWM, 255-((byte)abs(speed)));
break;
case 31:
bitWrite(PORTC, 6, 1); //PC6
PinMan.analogWrite(pinPWM, 255-((byte)abs(speed)));
break;
case 29:
bitWrite(PORTA, 7, 1); //PA7
PinMan.analogWrite(pinPWM, 255-((byte)abs(speed)));
break;
default:
// no other pin will work
break;
}
} else {
switch (pinDir) {
case 33:
bitWrite(PORTC, 4, 0); //PC4
PinMan.analogWrite(pinPWM, ((byte)speed));
break;
case 31:
bitWrite(PORTC, 6, 0); //PC6
PinMan.analogWrite(pinPWM, ((byte)speed));
break;
case 29:
bitWrite(PORTA, 7, 0); //PA7
PinMan.analogWrite(pinPWM, ((byte)speed));
break;
default:
// no other pin will work
break;
}
}
#else
Debug.println(F("not defined AVR in driver.cpp"))
if (speed < 0){
digitalWrite(pinDir, HIGH) ;
PinMan.analogWrite(pinPWM, 255-((byte)abs(speed)));
} else {
digitalWrite(pinDir, LOW) ;
PinMan.analogWrite(pinPWM, ((byte)speed));
}
#endif
}

// ---- sensor drivers --------------------------------------------------------------
Expand Down Expand Up @@ -220,13 +261,13 @@ unsigned int readURM37(int triggerPin, int echoPin){
boolean readDS1307(datetime_t &dt){
byte buf[8];
if (I2CreadFrom(DS1307_ADDRESS, 0x00, 8, buf, 3) != 8) {
Debug.println("DS1307 comm error");
Debug.println(F("DS1307 comm error"));
//addErrorCounter(ERR_RTC_COMM);
return false;
}
if ( ((buf[0] >> 7) != 0) || ((buf[1] >> 7) != 0) || ((buf[2] >> 7) != 0) || ((buf[3] >> 3) != 0)
|| ((buf[4] >> 6) != 0) || ((buf[5] >> 5) != 0) || ((buf[7] & B01101100) != 0) ) {
Debug.println("DS1307 data1 error");
Debug.println(F("DS1307 data1 error"));
//addErrorCounter(ERR_RTC_DATA);
return false;
}
Expand All @@ -240,7 +281,7 @@ boolean readDS1307(datetime_t &dt){
if ( (r.time.minute > 59) || (r.time.hour > 23) || (r.date.dayOfWeek > 6)
|| (r.date.month > 12) || (r.date.day > 31) || (r.date.day < 1)
|| (r.date.month < 1) || (r.date.year > 99) ){
Debug.println("DS1307 data2 error");
Debug.println(F("DS1307 data2 error"));
//addErrorCounter(ERR_RTC_DATA);
return false;
}
Expand All @@ -252,7 +293,7 @@ boolean readDS1307(datetime_t &dt){
boolean setDS1307(datetime_t &dt){
byte buf[7];
if (I2CreadFrom(DS1307_ADDRESS, 0x00, 7, buf, 3) != 7){
Debug.println("DS1307 comm error");
Debug.println(F("DS1307 comm error"));
//addErrorCounter(ERR_RTC_COMM);
return false;
}
Expand Down
67 changes: 34 additions & 33 deletions code/ardumower/motor.h
View file
Expand Up @@ -67,8 +67,8 @@ void Robot::setMotorMowRPMState(boolean motorMowRpmState){

// calculate map position by odometry sensors
void Robot::calcOdometry(){
if ((!odometryUse) || (millis() < nextTimeOdometry)) return;
nextTimeOdometry = millis() + 200;
if ((!odometryUse) || (currentMillis < nextTimeOdometry)) return;
nextTimeOdometry = currentMillis + 200;

static int lastOdoLeft = 0;
static int lastOdoRight = 0;
Expand All @@ -84,9 +84,9 @@ void Robot::calcOdometry(){
double wheel_theta = (left_cm - right_cm) / ((double)odometryWheelBaseCm);
odometryTheta += wheel_theta;

motorLeftRpmCurr = double ((( ((double)ticksLeft) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0);
motorRightRpmCurr = double ((( ((double)ticksRight) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0);
lastMotorRpmTime = millis();
motorLeftRpmCurr = double ((( ((double)ticksLeft) / ((double)odometryTicksPerRevolution)) / ((double)(currentMillis - lastMotorRpmTime))) * 60000.0);
motorRightRpmCurr = double ((( ((double)ticksRight) / ((double)odometryTicksPerRevolution)) / ((double)(currentMillis - lastMotorRpmTime))) * 60000.0);
lastMotorRpmTime = currentMillis;

if (imuUse){
odometryX += avg_cm * sin(imu.ypr.yaw);
Expand All @@ -104,8 +104,8 @@ void Robot::calcOdometry(){
// http://wiki.ardumower.de/images/a/a5/Motor_polarity_switch_protection.png
// - optional: ensures that the motors (and gears) are not switched to 0% (or 100%) too fast (motorAccel)
void Robot::setMotorPWM(int pwmLeft, int pwmRight, boolean useAccel){
unsigned long TaC = millis() - lastSetMotorSpeedTime; // sampling time in millis
lastSetMotorSpeedTime = millis();
unsigned long TaC = currentMillis - lastSetMotorSpeedTime; // sampling time in millis
lastSetMotorSpeedTime = currentMillis;
if (TaC > 1000) TaC = 1;

if (useAccel){
Expand Down Expand Up @@ -158,8 +158,8 @@ void Robot::setMotorPWM(int pwmLeft, int pwmRight, boolean useAccel){
// - ensures that the motor is not switched to 100% too fast (motorMowAccel)
// - ensures that the motor voltage is not higher than motorMowSpeedMaxPwm
void Robot::setMotorMowPWM(int pwm, boolean useAccel){
unsigned long TaC = millis() - lastSetMotorMowSpeedTime; // sampling time in millis
lastSetMotorMowSpeedTime = millis();
unsigned long TaC = currentMillis - lastSetMotorMowSpeedTime; // sampling time in millis
lastSetMotorMowSpeedTime = currentMillis;
if (TaC > 1000) TaC = 1;
// we need to ignore acceleration for PID control, and we can ignore if speed is lowered (e.g. motor is shut down)
if ( (!useAccel) || (pwm < motorMowPWMCurr) )
Expand All @@ -174,8 +174,8 @@ void Robot::setMotorMowPWM(int pwm, boolean useAccel){

// PID controller: roll robot to heading (requires IMU)
void Robot::motorControlImuRoll(){
if (millis() < nextTimeMotorImuControl) return;
nextTimeMotorImuControl = millis() + 100;
if (currentMillis < nextTimeMotorImuControl) return;
nextTimeMotorImuControl = currentMillis + 100;


// Regelbereich entspricht 80% der maximalen Drehzahl am Antriebsrad (motorSpeedMaxRpm)
Expand Down Expand Up @@ -211,7 +211,7 @@ void Robot::motorControlImuRoll(){
//if((motorRightSpeedRpmSet >= 0 ) && (rightSpeed <0 )) rightSpeed = 0;
//if((motorRightSpeedRpmSet <= 0 ) && (rightSpeed >0 )) rightSpeed = 0;

if ( ((stateCurr == STATE_OFF) || (stateCurr == STATE_STATION) || (stateCurr == STATE_ERROR)) && (millis()-stateStartTime>1000) ){
if ( ((stateCurr == STATE_OFF) || (stateCurr == STATE_STATION) || (stateCurr == STATE_ERROR)) && (currentMillis-stateStartTime>1000) ){
leftSpeed = rightSpeed = 0; // ensures PWM is zero if OFF/CHARGING
}
setMotorPWM( leftSpeed, rightSpeed, false );
Expand All @@ -222,8 +222,8 @@ void Robot::motorControlImuRoll(){

// PID controller: track perimeter
void Robot::motorControlPerimeter(){
if (millis() < nextTimeMotorPerimeterControl) return;
nextTimeMotorPerimeterControl = millis() + 30;
if (currentMillis < nextTimeMotorPerimeterControl) return;
nextTimeMotorPerimeterControl = currentMillis + 30;
if (trackInsidePerimeterOnly){
if (perimeterInside){
perimeterPID.x = (double(perimeterMagMedianInside.getMedian())/double(perimeterMagMax));
Expand All @@ -242,7 +242,7 @@ void Robot::motorControlPerimeter(){
if (lastPerimeterTrackInside) perimeterPID.reset();
lastPerimeterTrackInside = 0;
perimeterPID.y_min = -MaxSpeedperiPwm;
perimeterPID.y_max = MaxSpeedperiPwm;
perimeterPID.y_max = MaxSpeedperiPwm;
perimeterPID.max_output = MaxSpeedperiPwm;
perimeterPID.compute();
setMotorPWM( max(0, min(MaxSpeedperiPwm, MaxSpeedperiPwm/periTrackDivider - perimeterPID.y)),
Expand All @@ -253,7 +253,7 @@ void Robot::motorControlPerimeter(){
Console.println(perimeterPID.y); */
}
else{ //!trackInsidePerimeterOnly normal Line following
if ((millis() > stateStartTime + 5000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut)){
if ((currentMillis > stateStartTime + 5000) && (currentMillis > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut)){
// robot is wheel-spinning while tracking => roll to get ground again
if (trackingBlockInnerWheelWhilePerimeterStruggling == 0){
if (perimeterInside) setMotorPWM( -MaxSpeedperiPwm/2, MaxSpeedperiPwm/2, false);
Expand All @@ -264,7 +264,7 @@ void Robot::motorControlPerimeter(){
else setMotorPWM( MaxSpeedperiPwm/2, MaxSpeedperiPwm/6, false);
}

if (millis() > perimeterLastTransitionTime + trackingErrorTimeOut){
if (currentMillis > perimeterLastTransitionTime + trackingErrorTimeOut){
Debug.println("Error: tracking error");
addErrorCounter(ERR_TRACKING);
//setNextState(STATE_ERROR,0);
Expand All @@ -286,7 +286,7 @@ void Robot::motorControlPerimeter(){
//if (perimeterPID.x > 1) perimeterPID.x = 1;
//else if (perimeterPID.x < -1) perimeterPID.x = -1;
perimeterPID.y_min = -MaxSpeedperiPwm;
perimeterPID.y_max = MaxSpeedperiPwm;
perimeterPID.y_max = MaxSpeedperiPwm;
perimeterPID.max_output = MaxSpeedperiPwm;
perimeterPID.compute();
//setMotorPWM( motorLeftPWMCurr +perimeterPID.y,
Expand All @@ -301,8 +301,8 @@ void Robot::motorControlPerimeter(){

// PID controller: correct direction during normal driving (requires IMU)
void Robot::motorControlImuDir(){
if (millis() < nextTimeMotorImuControl) return;
nextTimeMotorImuControl = millis() + 100;
if (currentMillis < nextTimeMotorImuControl) return;
nextTimeMotorImuControl = currentMillis + 100;

int correctLeft = 0;
int correctRight = 0;
Expand Down Expand Up @@ -345,7 +345,7 @@ void Robot::motorControlImuDir(){
if((motorRightSpeedRpmSet >= 0 ) && (rightSpeed <0 )) rightSpeed = 0;
if((motorRightSpeedRpmSet <= 0 ) && (rightSpeed >0 )) rightSpeed = 0;

if ( ((stateCurr == STATE_OFF) || (stateCurr == STATE_STATION) || (stateCurr == STATE_ERROR)) && (millis()-stateStartTime>1000) ){
if ( ((stateCurr == STATE_OFF) || (stateCurr == STATE_STATION) || (stateCurr == STATE_ERROR)) && (currentMillis-stateStartTime>1000) ){
leftSpeed = rightSpeed = 0; // ensures PWM is zero if OFF/CHARGING
}
setMotorPWM( leftSpeed, rightSpeed, false );
Expand All @@ -357,12 +357,12 @@ void Robot::checkOdometryFaults(){
if (!odometryUse) return;
bool leftErr = false;
bool rightErr = false;
if ((stateCurr == STATE_FORWARD) && (millis()-stateStartTime>8000) ) {
if ((stateCurr == STATE_FORWARD) && (currentMillis-stateStartTime>8000) ) {
// just check if odometry sensors may not be working at all
if ( (motorLeftPWMCurr > 100) && (abs(motorLeftRpmCurr) < 1) ) leftErr = true;
if ( (motorRightPWMCurr > 100) && (abs(motorRightRpmCurr) < 1) ) rightErr = true;
}
if ((stateCurr == STATE_ROLL) && (millis()-stateStartTime>1000) ) {
if ((stateCurr == STATE_ROLL) && (currentMillis-stateStartTime>1000) ) {
// just check if odometry sensors may be turning in the wrong direction
if ( ((motorLeftPWMCurr > 100) && (motorLeftRpmCurr < -3)) || ((motorLeftPWMCurr < -100) && (motorLeftRpmCurr > 3)) ) leftErr = true;
if ( ((motorRightPWMCurr > 100) && (motorRightRpmCurr < -3)) || ((motorRightPWMCurr < -100) && (motorRightRpmCurr > 3)) ) rightErr = true;
Expand All @@ -387,8 +387,8 @@ void Robot::checkOdometryFaults(){


void Robot::motorControl(){
if (millis() < nextTimeMotorControl) return;
nextTimeMotorControl = millis() + 100;
if (currentMillis < nextTimeMotorControl) return;
nextTimeMotorControl = currentMillis + 100;
static unsigned long nextMotorControlOutputTime = 0;
if (odometryUse){
// Regelbereich entspricht maximaler PWM am Antriebsrad (motorSpeedMaxPwm), um auch an Steigungen h�chstes Drehmoment f�r die Solldrehzahl zu gew�hrleisten
Expand All @@ -401,7 +401,7 @@ void Robot::motorControl(){
motorRightPID.w = motorRightSpeedRpmSet + RLdiff/2;
}
motorLeftPID.x = motorLeftRpmCurr; // IST
if (millis() < stateStartTime + motorZeroSettleTime) motorLeftPID.w = 0; // get zero speed first after state change
if (currentMillis < stateStartTime + motorZeroSettleTime) motorLeftPID.w = 0; // get zero speed first after state change
motorLeftPID.y_min = -motorSpeedMaxPwm; // Regel-MIN
motorLeftPID.y_max = motorSpeedMaxPwm; // Regel-MAX
motorLeftPID.max_output = motorSpeedMaxPwm; // Begrenzung
Expand All @@ -416,7 +416,7 @@ void Robot::motorControl(){
motorRightPID.Ki = motorLeftPID.Ki;
motorRightPID.Kd = motorLeftPID.Kd;
motorRightPID.x = motorRightRpmCurr; // IST
if (millis() < stateStartTime + motorZeroSettleTime) motorRightPID.w = 0; // get zero speed first after state change
if (currentMillis < stateStartTime + motorZeroSettleTime) motorRightPID.w = 0; // get zero speed first after state change
motorRightPID.y_min = -motorSpeedMaxPwm; // Regel-MIN
motorRightPID.y_max = motorSpeedMaxPwm; // Regel-MAX
motorRightPID.max_output = motorSpeedMaxPwm; // Begrenzung
Expand All @@ -429,8 +429,8 @@ void Robot::motorControl(){
if ( (abs(motorLeftPID.x) < 2) && (abs(motorLeftPID.w) < 0.1) ) leftSpeed = 0; // ensures PWM is really zero
if ( (abs(motorRightPID.x) < 2) && (abs(motorRightPID.w) < 0.1) ) rightSpeed = 0; // ensures PWM is really zero

/*if (millis() >= nextMotorControlOutputTime){
nextMotorControlOutputTime = millis() + 3000;
/*if (currentMillis >= nextMotorControlOutputTime){
nextMotorControlOutputTime = currentMillis + 3000;
Console.print("PID x=");
Console.print(motorLeftPID.x);
Console.print("\tPID w=");
Expand All @@ -446,7 +446,7 @@ void Robot::motorControl(){
else{
int leftSpeed = min(motorSpeedMaxPwm, max(-motorSpeedMaxPwm, map(motorLeftSpeedRpmSet, -motorSpeedMaxRpm, motorSpeedMaxRpm, -motorSpeedMaxPwm, motorSpeedMaxPwm)));
int rightSpeed =min(motorSpeedMaxPwm, max(-motorSpeedMaxPwm, map(motorRightSpeedRpmSet, -motorSpeedMaxRpm, motorSpeedMaxRpm, -motorSpeedMaxPwm, motorSpeedMaxPwm)));
if (millis() < stateStartTime + motorZeroSettleTime) {
if (currentMillis < stateStartTime + motorZeroSettleTime) {
leftSpeed = rightSpeed = 0; // slow down at state start
if (mowPatternCurr != MOW_LANES) imuDriveHeading = imu.ypr.yaw; // set drive heading
}
Expand All @@ -459,9 +459,9 @@ void Robot::motorControl(){
// input: motorMowEnable, motorMowModulate, motorMowRpmCurr
// output: motorMowPWMCurr
void Robot::motorMowControl(){
if (millis() < nextTimeMotorMowControl) return;
if (currentMillis < nextTimeMotorMowControl) return;

nextTimeMotorMowControl = millis() + 100;
nextTimeMotorMowControl = currentMillis + 100;
if (motorMowEnableOverride) motorMowEnable = false;
double mowSpeed ;
if (!motorMowEnable) {
Expand Down Expand Up @@ -489,7 +489,7 @@ void Robot::motorMowControl(){
motorMowPID.x = 0.2* motorMowRpmCurr + 0.8 * motorMowPID.x;
motorMowPID.w = mowSpeed; // 3300 => 2300
motorMowPID.y_min = -motorMowSpeedMaxPwm/2;
motorMowPID.y_max = motorMowSpeedMaxPwm/2;
motorMowPID.y_max = motorMowSpeedMaxPwm/2;
motorMowPID.max_output = motorMowSpeedMaxPwm/2;
motorMowPID.compute();

Expand Down Expand Up @@ -520,3 +520,4 @@ void Robot::printOdometry(){
}



0 comments on commit cc81045

Please sign in to comment.