Project Overview: Option 2
For this project, you will modify your microcontroller circuit to extend your external command interface (right now, you have 1 switch to control the orientation display). We will use this command interface to trigger a sequence of height and orientation subgoals.In brief, your code must:
- command the heli to maintain the designated target orientation and target throttle,
- respond to new commands by changing the orientation and throttle targets in the specified manner (using a finite state machine),
- indicate the current state of your FSM using a set of 3 LEDs (you may use your existing LEDs for this).
Project Components
Part 1: Microcontroller Circuit
- Designate two digital input pins (you have one already). Call these C1 and C0 (you may choose any 2 pins).
- Connected these pins to +5V through a 10K ohm resistor (one resistor for each pin).
- Connect a switch from each of these pins to ground. The pin will read "0" or "1" depending on whether the switch has been pressed.
Part 2: Finite State Machine
Implement in software the following finite state machine:
- Assign each state a unique integer between 0 and 5
- Events:
- Numbers indicating events represent the state of the input command lines C1 and C0 (and "xx" means any value for each)
- th:goal -> throttle_goal is equal to throttle_target (within some tolerance)
- heading:goal -> heading_goal is equal to heading_target (within some tolerance)
- Actions:
- th:off -> set the throttle_target to off
- th:low -> set the throttle_target to a low level (high enough to barely hover)
- th:med -> set the throttle_target to a medium level (~50cm)
- th:high -> set the throttle_target to a high level (~100cm)
- heading:N -> set the heading_target to North
- heading:cur -> set the heading_target to the current heading
- heading +=90 -> change the heading_target by 90 degrees
- Display the state of the FSM using a set of 3 LEDs (you may re-use your LEDs)
Note: Develop and test FSM before you move on to the next part (you will have to fake the orientation and throttle goal events)
Part 3: Finite State Machines for Control
Control the craft using this FSM.Here is a starting point for your main loop:
void main_loop(void) {
int16_t heading_target = get_heading();
int16_t heading_goal = heading_target;
float throttle_target = 50.0; // Throttle off
float throttle_goal = throttle_target;
while(1) {
// Finite state machine control
switch(state)
{
case STATE_WAIT:
:
:
break;
case STATE_LAND:
:
:
break;
:
:
}
// Move the throttle goal a little closer to the target
throttle_goal += K_throttle * clip(throttle_target - throttle_goal, max_throttle_step);
// Move the heading goal a little closer to the target
diff = heading_target - heading_goal;
if(diff > 1800)
diff -= 3600;
if(diff < -1799)
diff += 3600;
heading_goal += K_heading * clip(diff, max_heading_step);
// Get current heading and compute PID control signals
heading_last = heading_current;
heading_current = get_heading();
heading_error = compute_error(heading_current, heading_goal);
heading_derivative = compute_derivative(heading_current, heading_last);
if(#COMMAND INPUT LINE#) {
display_orient(heading_current);
}else{
display_orient(heading_error);
}
display_derivative(heading_derivative);
yaw_command = yaw_center +
Kp * heading_error - Kv * heading_derivative;
set_yaw(yaw_command);
set_throttle((uint8_t) throttle_goal);
delay_ms(100);
}
}
/*
int16_t clip(int16_t error, int16_t max_error)
Clip the error to a value that is between -max_error and max_error
*/
int16_t clip(int16_t error, int16_t max_error)
{
if(error > max_error)
return(max_error)
if(error < -max_error)
return(-max_error)
return(error);
}