Coarse Alignment State Machine
Coarse alignment uses an ultrasonic range sensor (MB7052) mounted to a servo motor (Parallax Standard Servo) to estimate the rough position of a car in the parking space. The initialization for the state machine initializes these two peripherals and puts the state machine into an initial SET HOME RANGE state. When the main state machine calls the coarse alignment SM for the first time, the state machine will take a range reading to determine the distance of the vehicle when the sensor is pointed straight ahead (90 degrees).
The state machine then enters the FIND DRIVER SIDE state machine, sweeping clockwise until a discontinuous increase in the range reading has been seen. The range and angle are stored as (r1, theta1) to be used in the localization algorithm. The state machine then enters a GO BACK HOME state, which commands the servo to go back to 90 degrees. It then waits in this position until the URF reads the range that was seen in the SET HOME RANGE state.
Once the sensor has returned to the home angle, the state machine enters the FIND PASSENGER SIDE state. This time, it sweeps counter-clockwise until another positive discontinuity is seen. This range and angle are stored as (r2, theta2). At this point, the values are sent to the CoarseLocalization algorithm and the rough position of the secondary coil are calculated (see below). The platform is commanded to this position and the coarse alignment SM returns to the main SM to proceed with fine alignment.
If at any time the servo tries to move to an angle that is outside its range of motion (0 to 180 degrees), the state machine returns with a -1, signaling that coarse alignment failed.
If at any time a negative discontinuity is seen, something is blocking the range sensor and we wait until the obstacle is removed. At the moment this is blocking code, and if the range sensor fails to read the previous range before it was blocked, the code will continue to hang here. A timeout should be added here break out of this, indicate coarse alignment failure and return to the main SM.
The state machine then enters the FIND DRIVER SIDE state machine, sweeping clockwise until a discontinuous increase in the range reading has been seen. The range and angle are stored as (r1, theta1) to be used in the localization algorithm. The state machine then enters a GO BACK HOME state, which commands the servo to go back to 90 degrees. It then waits in this position until the URF reads the range that was seen in the SET HOME RANGE state.
Once the sensor has returned to the home angle, the state machine enters the FIND PASSENGER SIDE state. This time, it sweeps counter-clockwise until another positive discontinuity is seen. This range and angle are stored as (r2, theta2). At this point, the values are sent to the CoarseLocalization algorithm and the rough position of the secondary coil are calculated (see below). The platform is commanded to this position and the coarse alignment SM returns to the main SM to proceed with fine alignment.
If at any time the servo tries to move to an angle that is outside its range of motion (0 to 180 degrees), the state machine returns with a -1, signaling that coarse alignment failed.
If at any time a negative discontinuity is seen, something is blocking the range sensor and we wait until the obstacle is removed. At the moment this is blocking code, and if the range sensor fails to read the previous range before it was blocked, the code will continue to hang here. A timeout should be added here break out of this, indicate coarse alignment failure and return to the main SM.