First, a Disclaimer
- I freely admit that this code is not exactly pretty. However, it does work and is sufficiently bug free.
- It needs comments and should be refactored to use a class for the Points struct.
- It's not been code reviewed and I'd be happy for your gracious comments.
Overview
There are only 2 files - Points.h and the main Arduino sketch. I was going to work it all into one file but there are some (known) bugs in the Arduino IDE that forced me to move some of my typedefs into the header file. The bug in the IDE si to do with the fact that the main sketch file is not parsed in strict line number order (somehow!).
The basic concept for this software is that I have an array of Points each of which is controlled by a well defined state machine. The system is essentially a table driven finite state machine with the context of where each point is "up to" stored in the Point data structure.
Feel free to pinch this code.
Points.h
#ifndef POINTS_H
#define POINTS_H 1
/*
* Points.h - my arduino driven switch machine software….
*/
typedef enum { cal_st, cal_sw, moving_sw, moving_st,
idle_sw, idle_st
} PointState ;
typedef enum { cal_enter, cal_exit, mov_straight,
mov_switch, set_pt_changed, pt_tick
} PointEvent ;
typedef struct {
int ID;
int servo_pin;
int calib_pin;
int control_pin;
int set_pt_pin;
int eeprom_sw_addr;
int eeprom_st_addr;
int straight_angle;
int switch_angle;
int calib;
int control;
int set_point;
int current_angle;
int dest_angle;
PointState current_state;
Servo point_servo;
} Point;
typedef struct {
PointState current;
PointEvent event;
PointState next;
void (*transition)( Point* p);
} PointStateElement;
#endif POINTS_H
#define POINTS_H 1
/*
* Points.h - my arduino driven switch machine software….
*/
typedef enum { cal_st, cal_sw, moving_sw, moving_st,
idle_sw, idle_st
} PointState ;
typedef enum { cal_enter, cal_exit, mov_straight,
mov_switch, set_pt_changed, pt_tick
} PointEvent ;
typedef struct {
int ID;
int servo_pin;
int calib_pin;
int control_pin;
int set_pt_pin;
int eeprom_sw_addr;
int eeprom_st_addr;
int straight_angle;
int switch_angle;
int calib;
int control;
int set_point;
int current_angle;
int dest_angle;
PointState current_state;
Servo point_servo;
} Point;
typedef struct {
PointState current;
PointEvent event;
PointState next;
void (*transition)( Point* p);
} PointStateElement;
#endif POINTS_H
And the main Arduino Sketch
#include <Servo.h>
#include <EEPROM.h>
#include "<insert path>/Points.h"
#define NUM_POINTS 4
#define SVO_START_PIN 10 // The first pin to which a servo is connected
#define CTL_START_PIN 6 // The first pin to be used for controlling the points
#define CAL_START_PIN 2 // The first pin to be used for controlling calibration
#define SET_PT_PIN 0 // The analogue pin to be used for reading the set point
#define PT_EEPROM_ST 0 // The first address in the EEPROM for point settings to be kept
//#define SER_OUT 1 // If defined, serial debug output is compiled in
#define SERVO_DFT_ANG 90 // Where the servo will sit if it is uncalibrated
#define SERVO_MIN_ANG 70
#define SERVO_MAX_ANG 110
#define IsAngleValid(a) (((a) >= SERVO_MIN_ANG) && ((a) <= SERVO_MAX_ANG))
#define PT_POS_STRAIGHT 1
#define PT_POS_SWITCH 0
#define PT_CALIBRATE 0
#define PT_OPERATE 1
void
Points_error(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" error.");
#endif
}
void
Points_nop(Point* p)
{
}
void
Points_write_cal_st(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" writing calibration for straight.");
#endif
EEPROM.write(p->eeprom_st_addr , p->current_angle);
p->straight_angle = p->current_angle;
}
void
Points_match_set_pt(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" match set point.");
#endif
p->current_angle = p->set_point;
p->point_servo.write(p->set_point);
}
void
Points_write_cal_sw(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" writing calibration for switch.");
#endif
EEPROM.write(p->eeprom_sw_addr , p->current_angle);
p->switch_angle = p->current_angle;
}
void
Points_start_moving_st(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("start moving straight.");
#endif
if ( IsAngleValid(p->straight_angle) )
{
p->dest_angle = p->straight_angle;
}
else
{
p->point_servo.write(SERVO_DFT_ANG - 2);
p->point_servo.write(SERVO_DFT_ANG + 2);
p->point_servo.write(SERVO_DFT_ANG);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" straight is uncalibrated");
#endif
}
}
void
Points_start_moving_sw(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" start moving switch.");
#endif
if ( IsAngleValid(p->switch_angle) )
{
p->dest_angle = p->switch_angle;
}
else
{
p->point_servo.write(SERVO_DFT_ANG - 2);
p->point_servo.write(SERVO_DFT_ANG + 2);
p->point_servo.write(SERVO_DFT_ANG);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" switch is uncalibrated");
#endif
}
}
void
Points_move_step(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" move_step.");
#endif
if(p->current_angle < p->dest_angle)
{
p->current_angle ++;
}
else if(p->current_angle > p->dest_angle)
{
p->current_angle --;
}
p->point_servo.write(p->current_angle);
if(p->current_angle == p->dest_angle)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.print(" reached destination ");
Serial.print(p->dest_angle);
Serial.print("( ");
Serial.print(p->switch_angle);
Serial.print(", ");
Serial.print(p->straight_angle);
Serial.println(")");
#endif
if(p->dest_angle == p->switch_angle)
p->current_state = idle_sw;
else
p->current_state = idle_st;
}
}
PointStateElement PointStateTable[] = {
{cal_st, cal_enter, cal_st, Points_error },
{cal_st, cal_exit, idle_st, Points_write_cal_st },
{cal_st, mov_straight, cal_st, Points_error },
{cal_st, mov_switch, cal_sw, Points_nop },
{cal_st, set_pt_changed, cal_st, Points_match_set_pt },
{cal_st, pt_tick, cal_st, Points_nop },
{cal_sw, cal_enter, cal_sw, Points_error },
{cal_sw, cal_exit, idle_sw, Points_write_cal_sw },
{cal_sw, mov_straight, cal_st, Points_nop },
{cal_sw, mov_switch, cal_sw, Points_error },
{cal_sw, set_pt_changed, cal_sw, Points_match_set_pt },
{cal_sw, pt_tick, cal_sw, Points_nop },
{moving_sw, cal_enter, cal_sw, Points_match_set_pt },
{moving_sw, cal_exit, moving_sw, Points_error },
{moving_sw, mov_straight, moving_st, Points_start_moving_st},
{moving_sw, mov_switch, moving_sw, Points_error },
{moving_sw, set_pt_changed, moving_sw, Points_nop },
{moving_sw, pt_tick, moving_sw, Points_move_step },
{moving_st, cal_enter, cal_st, Points_match_set_pt },
{moving_st, cal_exit, moving_st, Points_error },
{moving_st, mov_straight, moving_st, Points_error },
{moving_st, mov_switch, moving_sw, Points_start_moving_sw},
{moving_st, set_pt_changed, moving_st, Points_nop },
{moving_st, pt_tick, moving_st, Points_move_step },
{idle_sw, cal_enter, cal_sw, Points_match_set_pt },
{idle_sw, cal_exit, idle_sw, Points_error },
{idle_sw, mov_straight, moving_st, Points_start_moving_st},
{idle_sw, mov_switch, idle_sw, Points_error },
{idle_sw, set_pt_changed, idle_sw, Points_nop },
{idle_sw, pt_tick, idle_sw, Points_nop },
{idle_st, cal_enter, cal_st, Points_match_set_pt },
{idle_st, cal_exit, idle_st, Points_error },
{idle_st, mov_straight, idle_st, Points_error },
{idle_st, mov_switch, moving_sw, Points_start_moving_sw},
{idle_st, set_pt_changed, idle_st, Points_nop },
{idle_st, pt_tick, idle_st, Points_nop }
};
#define NUM_STATES (sizeof(PointStateTable)/sizeof(PointStateElement))
int
PointInit( Point* p, int ID, int ser_pin, int cal_pin, int con_pin, int eeprom_addr, int set_pt_pin )
{
int val;
p->ID = ID;
p->servo_pin = ser_pin;
p->calib_pin = cal_pin;
p->control_pin = con_pin;
p->eeprom_sw_addr = eeprom_addr;
p->eeprom_st_addr = eeprom_addr + 1;
p->set_pt_pin = set_pt_pin;
p->point_servo.attach(ser_pin);
p->straight_angle = EEPROM.read(p->eeprom_st_addr);
p->switch_angle = EEPROM.read(p->eeprom_sw_addr);
pinMode(p->calib_pin, INPUT);
pinMode(p->control_pin, INPUT);
val = analogRead(p->set_pt_pin);
p->calib = PT_OPERATE;
p->control = PT_POS_STRAIGHT;
p->set_point = map(val, 0, 1023, SERVO_MIN_ANG, SERVO_MAX_ANG);
p->current_state = moving_st;
p->point_servo.write(SERVO_DFT_ANG);
p->current_angle = SERVO_DFT_ANG;
p->dest_angle = SERVO_DFT_ANG;
}
PointEvent
PointGetEvent( Point* p)
{
int setval;
int set_pt;
if(( p->calib == PT_CALIBRATE) && (digitalRead(p->calib_pin) == PT_OPERATE))
{
p->calib = PT_OPERATE;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("cal_exit");
#endif
return(cal_exit);
}
else if(( p->calib == PT_OPERATE) && (digitalRead(p->calib_pin) == PT_CALIBRATE))
{
p->calib = PT_CALIBRATE;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("cal_enter");
#endif
return(cal_enter);
}
if(( p->control == PT_POS_STRAIGHT) && (digitalRead(p->control_pin) == PT_POS_SWITCH))
{
p->control = PT_POS_SWITCH;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("mov_switch");
#endif
return(mov_switch);
}
else if(( p->control == PT_POS_SWITCH) && (digitalRead(p->control_pin) == PT_POS_STRAIGHT))
{
p->control = PT_POS_STRAIGHT;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("mov_straight");
#endif
return(mov_straight);
}
setval = analogRead(p->set_pt_pin);
set_pt = map(setval, 0, 1023, SERVO_MIN_ANG, SERVO_MAX_ANG);
if( p->set_point != set_pt)
{
p->set_point = set_pt;
delay(20);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.print("set_pt_changed: ");
Serial.println(set_pt);
#endif
return(set_pt_changed);
}
delay(20);
return ( pt_tick );
}
void
PointHandleEvent( Point* p, PointEvent e)
{
int i,j;
PointState cs;
cs = p->current_state;
i = 0;
while( i < NUM_STATES)
{
if ((PointStateTable[i].current == cs) && (PointStateTable[i].event == e))
{
p->current_state = PointStateTable[i].next;
(*PointStateTable[i].transition)(p);
i = NUM_STATES;
}
else
i++;
}
}
void
PointRun( Point* p)
{
PointEvent e;
e = PointGetEvent(p);
PointHandleEvent(p,e);
}
Point PointArray[NUM_POINTS];
#include <EEPROM.h>
#include "<insert path>/Points.h"
#define NUM_POINTS 4
#define SVO_START_PIN 10 // The first pin to which a servo is connected
#define CTL_START_PIN 6 // The first pin to be used for controlling the points
#define CAL_START_PIN 2 // The first pin to be used for controlling calibration
#define SET_PT_PIN 0 // The analogue pin to be used for reading the set point
#define PT_EEPROM_ST 0 // The first address in the EEPROM for point settings to be kept
//#define SER_OUT 1 // If defined, serial debug output is compiled in
#define SERVO_DFT_ANG 90 // Where the servo will sit if it is uncalibrated
#define SERVO_MIN_ANG 70
#define SERVO_MAX_ANG 110
#define IsAngleValid(a) (((a) >= SERVO_MIN_ANG) && ((a) <= SERVO_MAX_ANG))
#define PT_POS_STRAIGHT 1
#define PT_POS_SWITCH 0
#define PT_CALIBRATE 0
#define PT_OPERATE 1
void
Points_error(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" error.");
#endif
}
void
Points_nop(Point* p)
{
}
void
Points_write_cal_st(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" writing calibration for straight.");
#endif
EEPROM.write(p->eeprom_st_addr , p->current_angle);
p->straight_angle = p->current_angle;
}
void
Points_match_set_pt(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" match set point.");
#endif
p->current_angle = p->set_point;
p->point_servo.write(p->set_point);
}
void
Points_write_cal_sw(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" writing calibration for switch.");
#endif
EEPROM.write(p->eeprom_sw_addr , p->current_angle);
p->switch_angle = p->current_angle;
}
void
Points_start_moving_st(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("start moving straight.");
#endif
if ( IsAngleValid(p->straight_angle) )
{
p->dest_angle = p->straight_angle;
}
else
{
p->point_servo.write(SERVO_DFT_ANG - 2);
p->point_servo.write(SERVO_DFT_ANG + 2);
p->point_servo.write(SERVO_DFT_ANG);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" straight is uncalibrated");
#endif
}
}
void
Points_start_moving_sw(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" start moving switch.");
#endif
if ( IsAngleValid(p->switch_angle) )
{
p->dest_angle = p->switch_angle;
}
else
{
p->point_servo.write(SERVO_DFT_ANG - 2);
p->point_servo.write(SERVO_DFT_ANG + 2);
p->point_servo.write(SERVO_DFT_ANG);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" switch is uncalibrated");
#endif
}
}
void
Points_move_step(Point* p)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println(" move_step.");
#endif
if(p->current_angle < p->dest_angle)
{
p->current_angle ++;
}
else if(p->current_angle > p->dest_angle)
{
p->current_angle --;
}
p->point_servo.write(p->current_angle);
if(p->current_angle == p->dest_angle)
{
#ifdef SER_OUT
Serial.print(p->ID);
Serial.print(" reached destination ");
Serial.print(p->dest_angle);
Serial.print("( ");
Serial.print(p->switch_angle);
Serial.print(", ");
Serial.print(p->straight_angle);
Serial.println(")");
#endif
if(p->dest_angle == p->switch_angle)
p->current_state = idle_sw;
else
p->current_state = idle_st;
}
}
PointStateElement PointStateTable[] = {
{cal_st, cal_enter, cal_st, Points_error },
{cal_st, cal_exit, idle_st, Points_write_cal_st },
{cal_st, mov_straight, cal_st, Points_error },
{cal_st, mov_switch, cal_sw, Points_nop },
{cal_st, set_pt_changed, cal_st, Points_match_set_pt },
{cal_st, pt_tick, cal_st, Points_nop },
{cal_sw, cal_enter, cal_sw, Points_error },
{cal_sw, cal_exit, idle_sw, Points_write_cal_sw },
{cal_sw, mov_straight, cal_st, Points_nop },
{cal_sw, mov_switch, cal_sw, Points_error },
{cal_sw, set_pt_changed, cal_sw, Points_match_set_pt },
{cal_sw, pt_tick, cal_sw, Points_nop },
{moving_sw, cal_enter, cal_sw, Points_match_set_pt },
{moving_sw, cal_exit, moving_sw, Points_error },
{moving_sw, mov_straight, moving_st, Points_start_moving_st},
{moving_sw, mov_switch, moving_sw, Points_error },
{moving_sw, set_pt_changed, moving_sw, Points_nop },
{moving_sw, pt_tick, moving_sw, Points_move_step },
{moving_st, cal_enter, cal_st, Points_match_set_pt },
{moving_st, cal_exit, moving_st, Points_error },
{moving_st, mov_straight, moving_st, Points_error },
{moving_st, mov_switch, moving_sw, Points_start_moving_sw},
{moving_st, set_pt_changed, moving_st, Points_nop },
{moving_st, pt_tick, moving_st, Points_move_step },
{idle_sw, cal_enter, cal_sw, Points_match_set_pt },
{idle_sw, cal_exit, idle_sw, Points_error },
{idle_sw, mov_straight, moving_st, Points_start_moving_st},
{idle_sw, mov_switch, idle_sw, Points_error },
{idle_sw, set_pt_changed, idle_sw, Points_nop },
{idle_sw, pt_tick, idle_sw, Points_nop },
{idle_st, cal_enter, cal_st, Points_match_set_pt },
{idle_st, cal_exit, idle_st, Points_error },
{idle_st, mov_straight, idle_st, Points_error },
{idle_st, mov_switch, moving_sw, Points_start_moving_sw},
{idle_st, set_pt_changed, idle_st, Points_nop },
{idle_st, pt_tick, idle_st, Points_nop }
};
#define NUM_STATES (sizeof(PointStateTable)/sizeof(PointStateElement))
int
PointInit( Point* p, int ID, int ser_pin, int cal_pin, int con_pin, int eeprom_addr, int set_pt_pin )
{
int val;
p->ID = ID;
p->servo_pin = ser_pin;
p->calib_pin = cal_pin;
p->control_pin = con_pin;
p->eeprom_sw_addr = eeprom_addr;
p->eeprom_st_addr = eeprom_addr + 1;
p->set_pt_pin = set_pt_pin;
p->point_servo.attach(ser_pin);
p->straight_angle = EEPROM.read(p->eeprom_st_addr);
p->switch_angle = EEPROM.read(p->eeprom_sw_addr);
pinMode(p->calib_pin, INPUT);
pinMode(p->control_pin, INPUT);
val = analogRead(p->set_pt_pin);
p->calib = PT_OPERATE;
p->control = PT_POS_STRAIGHT;
p->set_point = map(val, 0, 1023, SERVO_MIN_ANG, SERVO_MAX_ANG);
p->current_state = moving_st;
p->point_servo.write(SERVO_DFT_ANG);
p->current_angle = SERVO_DFT_ANG;
p->dest_angle = SERVO_DFT_ANG;
}
PointEvent
PointGetEvent( Point* p)
{
int setval;
int set_pt;
if(( p->calib == PT_CALIBRATE) && (digitalRead(p->calib_pin) == PT_OPERATE))
{
p->calib = PT_OPERATE;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("cal_exit");
#endif
return(cal_exit);
}
else if(( p->calib == PT_OPERATE) && (digitalRead(p->calib_pin) == PT_CALIBRATE))
{
p->calib = PT_CALIBRATE;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("cal_enter");
#endif
return(cal_enter);
}
if(( p->control == PT_POS_STRAIGHT) && (digitalRead(p->control_pin) == PT_POS_SWITCH))
{
p->control = PT_POS_SWITCH;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("mov_switch");
#endif
return(mov_switch);
}
else if(( p->control == PT_POS_SWITCH) && (digitalRead(p->control_pin) == PT_POS_STRAIGHT))
{
p->control = PT_POS_STRAIGHT;
delay(20); //debounce it
#ifdef SER_OUT
Serial.print(p->ID);
Serial.println("mov_straight");
#endif
return(mov_straight);
}
setval = analogRead(p->set_pt_pin);
set_pt = map(setval, 0, 1023, SERVO_MIN_ANG, SERVO_MAX_ANG);
if( p->set_point != set_pt)
{
p->set_point = set_pt;
delay(20);
#ifdef SER_OUT
Serial.print(p->ID);
Serial.print("set_pt_changed: ");
Serial.println(set_pt);
#endif
return(set_pt_changed);
}
delay(20);
return ( pt_tick );
}
void
PointHandleEvent( Point* p, PointEvent e)
{
int i,j;
PointState cs;
cs = p->current_state;
i = 0;
while( i < NUM_STATES)
{
if ((PointStateTable[i].current == cs) && (PointStateTable[i].event == e))
{
p->current_state = PointStateTable[i].next;
(*PointStateTable[i].transition)(p);
i = NUM_STATES;
}
else
i++;
}
}
void
PointRun( Point* p)
{
PointEvent e;
e = PointGetEvent(p);
PointHandleEvent(p,e);
}
Point PointArray[NUM_POINTS];
void setup()
{
int i;
#ifdef SER_OUT
Serial.begin(9600);
#endif
for ( i = 0; i < NUM_POINTS; i++)
{
PointInit(&PointArray[i], i, SVO_START_PIN + i, CAL_START_PIN + i, CTL_START_PIN + i, PT_EEPROM_ST + i*2, SET_PT_PIN);
}
}
void loop()
{
int i;
for ( i = 0; i < NUM_POINTS; i++)
{
PointRun(&PointArray[i]);
}
}
No comments:
Post a Comment