Consideration: Major code rewrite for AzuritBer to simplify and improve functionality

[Geo-Ron]

I have spend the last week going through your code and I am really impressed on the motor control with odometry. I do not fully grasp the inner workings of the PID controller and some of the calculations you make, but that will come eventually 😉

I have seen that it has become a really complex state-machine, because of the old Azurit code remains and that you have created a new state for every operation/target. In my opinion this makes everything over-complex and hard to improve.

I am proposing a new/revised way of coding the Ardumower, that I want to contribute to your current code.

I would like to know your point of view on this method and would really like to accomplish this together, instead of creating my own standalone fork of your code. So please reply here and discuss.

Change the way the states are used

My thoughts would be that we use the following items:

• A machine state defines/shows the robot action it is performing.
  • This would be, driving forward, reversing, turning, charging, etc
  • These simpeler robot actions should be no more than the robot can do and would re-used on a lot of statuses/tasks
• A status would be the job or main action the robot is performing
  • This would be; back to station, mowing, charging, etc
• A task would be a task within the status or job that the robot needs to perform to accomplish the job (or status)
  • This would be: turning at the end of a lane or on the perimeter, or avoid an obstacle
  • A task would be a combination of several states, like reverse when hit an obstacle and drive a circle around is to avoid it.

In the code it would be wise (to my opinion) to honour the following rules:

• every check on sensors would only return a variable about their status
  • and not change the state directly
• in the loop section of the state these variables would be evalutated and change a task according according to their inputs
• in the setState section for the new state, the output/motor settings would be applied/calculated
  • and preferably nothing more.

My recommendations on the state improvements would be the following

States (these would be the actual action the robot is doing; the outputs)

• OFF
  • Do nothing, except monitor battery
  • Set all motors to off
• DRIVE_FORWARD
  • Keep a straight heading, using the IMU and odometry
  • motorControlOdo();
  • Sonar distance ≤ sonarTriggerBelow would cause slower speed
  • near perimeter would cause slower speed if set in config
• STOP_HARD
  • Need to brute stop
  • Triggers:
  • Bumper
  • Sonar (at distance ≤ sonarToFrontDist*1.2)
  • Drive Motor Current ≥ motorPowerMax*.8
  • Next action based on trigger and status/task
• STOP_ROLLOUT
  • Gradually roll out the defined cm untill a stop.
  • Independed of the forward or reverse status
  • Triggers:
  • Perimeter found
  • Sonar (at distance ≤ (sonarToFrontDist+DistPeriOutStop)*1.2)
  • NeedToCalibrateIMU
  • BatteryLow
  • Next action, based on trigger
• ROLL
  • Turn the defined angle, number of degrees
  • If outside perimeter, retry (or revert) the roll
• ROLL_TO_HEADING
  • Turn untill the defined heading has been reached
• DRIVE_BACKWARDS
  • Drive back the defined cm
  • Based on previous state?
• PERI_FIND --> Should not be implemented. This would be FORWARD with task PERI_FIND and status BACK_TO_STATION
  • Drive straight untill the perimeter is detected
  • Direction possibly by GPS location
• PERI_TRACK
  • Track the perimeter using motorControlPerimeter();
• DRIVE_CIRCLE (better ARC)
  • this would be roll while driving forward (maybe backwards also?)
  • reason is to avoid a obstacle.
  • circle has a variable radius
  • End state depends on originating reason (task)
• MANUAL
  • uses motorControl(); with direct rpm settings from PFOD
• REMOTE --> Unable to find code that supports this
  • uses motorControl(); with direct steering/driving settings from RC
• MOTOR_CALIB_SPEED
• MOTOR_TEST

Statuses (what major job am I doing)

• NORMAL_MOWING
• SPIRAL_MOWING (High Grass circular cutting)
• WIRE_MOWING
• BACK_TO_STATION
• CHARGING
• WAITING
• TESTING
  • MOTOR TEST
  • Calibration from menu
• MANUAL
  • In RC or Manual
• DRIVE_TO_START
  • Leave station and track to start
  • Can include drive to other area

Task (current task/substatus)

This task would consist of a few predefined STATES.

For example, avoid obstacle would be reverse, roll x degrees, circles around it for x meter, roll back to original heading and resume status.

• DRIVE
  • Go forward and keep the heading
• ESCAPE_SMALL_AREA
  • Used to find an escape route from an small corner or corridor.
  • Go forward/backwards small distance
  • Is triggered by motorCurrent at reverse (could also be back bumper/sonar) or hit perimeter/obstacle really quick after turning
  • Rotate small angle and try to move forward a greater distance than the mower has reversed.
  • repeat if unable to travel a greater distance
• PERI_FIND
  • Heading can be random or set by heading to GPS coördinate.
  • Possibly rotate (ROLL) to find stronger perimeter signal and drive into that direction
• PERI_TRACK
  • Drive along the perimeter until charge detected
  • or Bumper if bumper at station
• PERI_READ
  • Turn small angles (45°) left and right, to find out if we can continue driving with a small (temporary) angle correction/ ARC driving
  • After peri out roll, reverse DistPeriOutStop*.75 and then check
  • Returns PERI location at left, right or center
• TURN
  • Change heading when reached the perimeter or unable to avoid obstacle
  • Angle would depend on mowing pattern
  • LANES: change lane (eventually 180°)
  • RANDOM: angle based on driven distance since last state change (see https://www.facebook.com/groups/319588508137220/permalink/4311102302319134)
    • Longer distance, greater angle. Set for the next 20 times
    • Short distance, smaller angle. Set for the next 20 times
• AVOID_OBSTACLE
  • Try to avoid obstacle and continue course
  • Try to circle around the object and continue
  • If bump on object or perimeter the second time, goto TURN and continue the original heading minus 180° (opposite direction)
• LEAVE_STATION
  • Need support for non-blocking station?
• TRACK_TO_START
  • Find and follow perimeter wire until the specified distance has been travelled
• DRIVE_TO_NEW_AREA
  • Drive into the specified heading for the specified distance
• WAIT_FOR_SIG
  • Stand still and wait until the perimeter signal is detected (greater than minimum smag big area)
  • If found, start PERI_FIND
• CALIBRATE
  • Stop and calibrate the IMU/COMPASS/GPS heading...
  • After success continue

You have to think about the states the other way around.

The used of only technical machine states instead of functional will reduced the amount of code that needs to be maintained because of less duplicate code.

A few examples:

- Job/Status: NORMAL_MOWING
  - Task: DRIVE
  - Machine state: FORWARD

- Job/Status: NORMAL_MOWING
  - Task: TURN (because of perimeter reached, while mowing random)
  - Machine state: STOP_ROLLOUT, then DRIVE_BACKWARDS, then ROLL for random°, then DRIVE

- Job/Status: NORMAL_MOWING
  - Task: TURN (because of perimeter reached, while mowing lanes)
  - Machine state: STOP_ROLLOUT, then DRIVE_BACKWARDS, then ROLL for 135°, then DRIVE 20cm, then ROLL for 45°, then DRIVE

- Job/Status: NORMAL_MOWING
  - Task: AVOID_OBSTACLE (because of bumper or sonar triggered)
  - Machine state: STOP_HARD, then DRIVE_BACKWARDS, then ROLL for 45°, then CIRCLE around is, then ROLL to original heading, then DRIVE

- Job/Status: BACK_TO_STATION
  - Task: PERI_FIND
  - Machine state: ROLL to find strongest PERIMETER heading (or to set GPS coördinates), then DRIVE untill perimeter OUTSIDE

- Job/Status: BACK_TO_STATION
  - Task: PERI_TRACK
  - Machine state: ROLL to get Perimeter INSIDE, then PERI_TRACK untill found station by chgVoltage

I know you have abandoned the GPS usage, but I would like to use it for a fast_track to go back to station. Only a first drive heading to find the perimeter.

A DEV branch would be a good idea.

[Geo-Ron]

Implementatie:

• Status wordt de feitelijke werkzaamheid waar de robot mee bezig is.
• Task wordt de taak die afhankelijk van de omgeving bepaald welke acties (States) er opvolgend gedaan dienen te worden.

Task wordt een nieuw array met de volgende state objecten (struct) met de volgende velden:

• State
• DistanceRobot
• DistanceWheelLeft
• DistanceWheelRight
• SpeedWheelLeft
• SpeedWheelRight
• Heading (change to heading if given) -> set to 720 if not used
• Angle (change an angle if given) -> set to 720 if not used
• Result (Unset, Success, Failure)
• ResultTrigger (None,Perimeter,Sonar,Bumper,motorCurrent,mowerCurrent,Tilt,GPS,highGrass)
• ActualDistanceWheelLeft
• ActualDistanceWheelRight

Deze array met states in een Task wordt aan het begin gevuld (setNewTask) Wanneer een state klaar is in een task, wordt Result=Success en kan setNewState de volgende staat beginnen. Deze wordt aangeroepen via getNewState.

Bij Result=Failure wordt ActualDistance gevuld, en een rollBack gestart op basis van deze data.

Dus:

newStatus change to NORMALMOW

1. setNewTask -> resultaat: Drive, want geen trigger voor andere actie
2. requestNextState -> FORWARD: SpeedWheelLeft = SpeedWheelRight = maxPWM
3. state FORWARD zal accelereren als de PWM < dan beoogd

bumperLeft triggered (in state routine)

1. setNewState: STOP_HARD
2. setNewTask -> resultaat: OBSTACLE_AVOID, want trigger bumperLeft
3. setNewTask -> array tasks met states wordt gevuld:
   • reverse 10 cm
   • rotate 90deg right (want bumper links),
   • drive arc of 1m (calculate wheel speed and distance based on robot dimensions),
   • rotate 90 deg right.
4. setNewTask start state via setNewState op index 0, via var currStatefromTask
5. loop op currTask bij success zet tasks[currIndex].Result = Success bij afleggen van de gedefinieerde afstand en roept requestNextState aan met tasks[currStatefromTask]
6. requestNextState hoogt currStatefromTask op met 1 bij success en stuurt dit naar setNextState
   • requestNextState start rollBack bij Failure, door aanroepen van huidige state naar setNewTask met boolean rollBack
   • setNewTask met rollback wordt speed en distance *-1 gedaan
   • aan einde gaat setNewTask weer naar requestNextState met rolledBack boolean
   • rolledBack boolean maakt retry met grotere arc diameter en retry = 1

Obstacle wordt ook een nieuw object (struct) om in kaart te brengen op basis van behaalde resultaten

Obstacle:

• Cause (None,Perimeter,Sonar,Bumper,motorCurrent,mowerCurrent,Tilt,GPS,highGrass)
• IsLeft
• IsRight

Met het obstacle kan bepaald worden of de linker bumper (bijvoorbeeld) deze ontdekt heeft. Dan kan met een obstacle_read ook gekeken worden of rechts ook een object te vinden is. Afhankelijk van de input kan worden bepaald wat de actie is.

Voorbeeld: