state_machine_modes25_11_11

Users » Nurbol » Code » state_machine_modes25_11_11

Nurbol Nurdaulet / Mbed 2 deprecated state_machine_modes25_11_11

Dependencies: mbed WattBob_TextLCD globals

main.cpp@1:a91950a8e20f, 2011-11-25 (annotated)

Committer:: Nurbol
Date:: Fri Nov 25 17:22:31 2011 +0000
Revision:: 1:a91950a8e20f
Parent:: 0:d8528fbbaf9c
Child:: 2:ea2d1441d485

Who changed what in which revision?

User	Revision	Line number	New contents of line
Nurbol	0:d8528fbbaf9c	1	//
Nurbol	0:d8528fbbaf9c	2	// COM_test : program to communicate via a COM port to a PC/laptop
Nurbol	0:d8528fbbaf9c	3	// ========
Nurbol	0:d8528fbbaf9c	4	//
Nurbol	0:d8528fbbaf9c	5	// Description
Nurbol	0:d8528fbbaf9c	6	// Program to receive ASCII format commands through a virtual COM port and
Nurbol	0:d8528fbbaf9c	7	// after executing the command return some data (optional, depending on
Nurbol	0:d8528fbbaf9c	8	// the command) and a status value.
Nurbol	0:d8528fbbaf9c	9	// The program understands two possible commands :
Nurbol	0:d8528fbbaf9c	10	// "s i j" : move servo 'i (0 to 5) to postion 'j' (0 to 90)
Nurbol	0:d8528fbbaf9c	11	// "r" : read the 'i' parameter of the last "s" command
Nurbol	0:d8528fbbaf9c	12	//
Nurbol	0:d8528fbbaf9c	13	// Version : 1.0
Nurbol	0:d8528fbbaf9c	14	// Author : Jim Herd
Nurbol	0:d8528fbbaf9c	15	// Date : 5th Oct 2011
Nurbol	0:d8528fbbaf9c	16	//
Nurbol	0:d8528fbbaf9c	17	#include "mbed.h"
Nurbol	0:d8528fbbaf9c	18	#include "MCP23017.h"
Nurbol	0:d8528fbbaf9c	19	#include "WattBob_TextLCD.h"
Nurbol	0:d8528fbbaf9c	20	#include "cmd_io.h"
Nurbol	0:d8528fbbaf9c	21	#include "globals.h"
Nurbol	0:d8528fbbaf9c	22
Nurbol	1:a91950a8e20f	23
Nurbol	0:d8528fbbaf9c	24
Nurbol	0:d8528fbbaf9c	25	//******************************************************************************
Nurbol	0:d8528fbbaf9c	26	//declare ticker
Nurbol	0:d8528fbbaf9c	27	//
Nurbol	0:d8528fbbaf9c	28	Ticker timer1p;
Nurbol	0:d8528fbbaf9c	29	Ticker timer2p;
Nurbol	0:d8528fbbaf9c	30	Ticker timercounter1p;
Nurbol	0:d8528fbbaf9c	31	Ticker timercounter2p;
Nurbol	0:d8528fbbaf9c	32	Ticker timerstatemachine;
Nurbol	0:d8528fbbaf9c	33
Nurbol	0:d8528fbbaf9c	34	//******************************************************************************
Nurbol	0:d8528fbbaf9c	35	//declare Pin
Nurbol	0:d8528fbbaf9c	36	//
Nurbol	0:d8528fbbaf9c	37	AnalogIn sensor1(p15);
Nurbol	0:d8528fbbaf9c	38	AnalogIn sensor2(p16);
Nurbol	0:d8528fbbaf9c	39	DigitalOut valueLED1(p23);
Nurbol	0:d8528fbbaf9c	40	DigitalOut valueLED2(p25);
Nurbol	0:d8528fbbaf9c	41
Nurbol	0:d8528fbbaf9c	42
Nurbol	0:d8528fbbaf9c	43	DigitalOut led1(LED1);
Nurbol	0:d8528fbbaf9c	44	DigitalOut led2(LED2);
Nurbol	0:d8528fbbaf9c	45	DigitalOut led3(LED3);
Nurbol	0:d8528fbbaf9c	46	DigitalOut led4(LED4);
Nurbol	0:d8528fbbaf9c	47
Nurbol	0:d8528fbbaf9c	48	DigitalOut clk(p24);
Nurbol	0:d8528fbbaf9c	49
Nurbol	0:d8528fbbaf9c	50	DigitalIn counter1p(p5);
Nurbol	0:d8528fbbaf9c	51	DigitalIn counter2p(p6);
Nurbol	0:d8528fbbaf9c	52	DigitalIn SW1p(p11);
Nurbol	0:d8528fbbaf9c	53	DigitalIn SW2p(p12);
Nurbol	0:d8528fbbaf9c	54
Nurbol	0:d8528fbbaf9c	55	//******************************************************************************
Nurbol	0:d8528fbbaf9c	56	//declare variable
Nurbol	0:d8528fbbaf9c	57	//
Nurbol	0:d8528fbbaf9c	58	bool Position1_1p;
Nurbol	0:d8528fbbaf9c	59	bool Position2_1p;
Nurbol	0:d8528fbbaf9c	60	bool Position1_2p;
Nurbol	0:d8528fbbaf9c	61	bool Position2_2p;
Nurbol	0:d8528fbbaf9c	62
Nurbol	0:d8528fbbaf9c	63	bool bSort_Mode; //to use in the code to define the mode
Nurbol	0:d8528fbbaf9c	64	bool bMaint_Mode;//to use in the code to define the mode
Nurbol	0:d8528fbbaf9c	65
Nurbol	0:d8528fbbaf9c	66	int state;
Nurbol	0:d8528fbbaf9c	67
Nurbol	0:d8528fbbaf9c	68
Nurbol	0:d8528fbbaf9c	69
Nurbol	0:d8528fbbaf9c	70	//******************************************************************************
Nurbol	0:d8528fbbaf9c	71	// declare functions
Nurbol	0:d8528fbbaf9c	72	//
Nurbol	0:d8528fbbaf9c	73	void sensor1p (void);
Nurbol	0:d8528fbbaf9c	74	void sensor2p (void);
Nurbol	0:d8528fbbaf9c	75	void counter1 (void);
Nurbol	0:d8528fbbaf9c	76	void counter2 (void);
Nurbol	0:d8528fbbaf9c	77	void state_machine (void);
Nurbol	0:d8528fbbaf9c	78	//
Nurbol	0:d8528fbbaf9c	79	//
Nurbol	0:d8528fbbaf9c	80	// 2. five servo outputs
Nurbol	0:d8528fbbaf9c	81	//
Nurbol	0:d8528fbbaf9c	82
Nurbol	0:d8528fbbaf9c	83	PwmOut servo_0(p26);
Nurbol	0:d8528fbbaf9c	84	//PwmOut servo_1(p25);
Nurbol	0:d8528fbbaf9c	85	//PwmOut servo_2(p24);
Nurbol	0:d8528fbbaf9c	86	//PwmOut servo_3(p23);
Nurbol	0:d8528fbbaf9c	87	PwmOut servo_4(p22);
Nurbol	0:d8528fbbaf9c	88	PwmOut servo_5(p21);
Nurbol	0:d8528fbbaf9c	89
Nurbol	0:d8528fbbaf9c	90	//
Nurbol	0:d8528fbbaf9c	91	// 3. objects necessary to use the 2*16 character MBED display
Nurbol	0:d8528fbbaf9c	92	//
Nurbol	0:d8528fbbaf9c	93	MCP23017 *par_port;
Nurbol	0:d8528fbbaf9c	94	WattBob_TextLCD *lcd;
Nurbol	0:d8528fbbaf9c	95	//
Nurbol	0:d8528fbbaf9c	96	// 4. Virtual COM port over USB link to laptop/PC
Nurbol	0:d8528fbbaf9c	97	//
Nurbol	0:d8528fbbaf9c	98	Serial pc(USBTX, USBRX);
Nurbol	0:d8528fbbaf9c	99
Nurbol	0:d8528fbbaf9c	100	//******************************************************************************
Nurbol	0:d8528fbbaf9c	101	// Defined GLOBAL variables and structures
Nurbol	0:d8528fbbaf9c	102	//
Nurbol	0:d8528fbbaf9c	103	CMD_STRUCT ext_cmd, commandcounter2; // structure to hold command data
Nurbol	0:d8528fbbaf9c	104	STAT_STRUCT ext_stat; // structure to hold status reply
Nurbol	0:d8528fbbaf9c	105
Nurbol	0:d8528fbbaf9c	106
Nurbol	0:d8528fbbaf9c	107	//CMD_STRUCT commandcounter2;
Nurbol	0:d8528fbbaf9c	108
Nurbol	0:d8528fbbaf9c	109	uint32_t last_servo; // store for last servo number
Nurbol	0:d8528fbbaf9c	110
Nurbol	0:d8528fbbaf9c	111	//************************************************************************
Nurbol	0:d8528fbbaf9c	112	//************************************************************************
Nurbol	0:d8528fbbaf9c	113	// init_sys : initialise the system
Nurbol	0:d8528fbbaf9c	114	// ========
Nurbol	0:d8528fbbaf9c	115	//
Nurbol	0:d8528fbbaf9c	116	// 1. Configure 2*16 character display
Nurbol	0:d8528fbbaf9c	117	// 2. Print "COM test" string
Nurbol	0:d8528fbbaf9c	118	// 3. initialise relevant global variables
Nurbol	0:d8528fbbaf9c	119	// 4. set COM port baud rate to 19200 bits per second
Nurbol	0:d8528fbbaf9c	120	//
Nurbol	0:d8528fbbaf9c	121	void init_sys(void) {
Nurbol	0:d8528fbbaf9c	122
Nurbol	0:d8528fbbaf9c	123	par_port = new MCP23017(p9, p10, 0x40);
Nurbol	0:d8528fbbaf9c	124	lcd = new WattBob_TextLCD(par_port);
Nurbol	0:d8528fbbaf9c	125
Nurbol	0:d8528fbbaf9c	126	par_port->write_bit(1,BL_BIT); // turn LCD backlight ON
Nurbol	0:d8528fbbaf9c	127	lcd->cls();
Nurbol	0:d8528fbbaf9c	128	lcd->locate(0,0);
Nurbol	0:d8528fbbaf9c	129	lcd->printf("W3C: Wheel Coin Counter Company");
Nurbol	0:d8528fbbaf9c	130
Nurbol	0:d8528fbbaf9c	131	servo_0.period(0.020); // servo requires a 20ms period, common for all 5 servo objects
Nurbol	0:d8528fbbaf9c	132	last_servo = SERVO_UNKNOWN;
Nurbol	0:d8528fbbaf9c	133	pc.baud(19200);
Nurbol	0:d8528fbbaf9c	134
Nurbol	0:d8528fbbaf9c	135	servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);
Nurbol	0:d8528fbbaf9c	136	servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);
Nurbol	0:d8528fbbaf9c	137
Nurbol	0:d8528fbbaf9c	138	Position1_1p = 1;
Nurbol	0:d8528fbbaf9c	139	Position2_1p = 0;
Nurbol	0:d8528fbbaf9c	140	Position1_2p = 1;
Nurbol	0:d8528fbbaf9c	141	Position2_2p = 0;
Nurbol	0:d8528fbbaf9c	142
Nurbol	0:d8528fbbaf9c	143	state = 0;
Nurbol	0:d8528fbbaf9c	144
Nurbol	0:d8528fbbaf9c	145	return;
Nurbol	0:d8528fbbaf9c	146	} // end init_sys
Nurbol	0:d8528fbbaf9c	147
Nurbol	0:d8528fbbaf9c	148	//************************************************************************
Nurbol	0:d8528fbbaf9c	149	// process_cmd : decode and execute command
Nurbol	0:d8528fbbaf9c	150	// ===========
Nurbol	0:d8528fbbaf9c	151	uint32_t process_cmd(CMD_STRUCT *command)
Nurbol	0:d8528fbbaf9c	152	{
Nurbol	0:d8528fbbaf9c	153	int32_t pulse_width;
Nurbol	0:d8528fbbaf9c	154
Nurbol	0:d8528fbbaf9c	155	switch (command->cmd_code) {
Nurbol	0:d8528fbbaf9c	156	//
Nurbol	0:d8528fbbaf9c	157	// move a servo
Nurbol	0:d8528fbbaf9c	158	//
Nurbol	0:d8528fbbaf9c	159	case SERVO_CMD :
Nurbol	0:d8528fbbaf9c	160	command->nos_data = 0; // no data to be returned
Nurbol	0:d8528fbbaf9c	161	//
Nurbol	0:d8528fbbaf9c	162	// check that parameters are OK
Nurbol	0:d8528fbbaf9c	163	//
Nurbol	0:d8528fbbaf9c	164	if (command->nos_params != 2) { // check for 2 parameters
Nurbol	0:d8528fbbaf9c	165	command->result_status = CMD_BAD_NUMBER_OF_PARAMETERS;
Nurbol	0:d8528fbbaf9c	166	break;
Nurbol	0:d8528fbbaf9c	167	}
Nurbol	0:d8528fbbaf9c	168	if (command->param[0] > MAX_SERVO_NUMBER ) { // check servo number
Nurbol	0:d8528fbbaf9c	169	command->result_status = CMD_BAD_SERVO_NUMBER;
Nurbol	0:d8528fbbaf9c	170	break;
Nurbol	0:d8528fbbaf9c	171	}
Nurbol	0:d8528fbbaf9c	172	if ((command->param[1] < MIN_SERVO_ANGLE) \|\|
Nurbol	0:d8528fbbaf9c	173	(command->param[1] > MAX_SERVO_ANGLE) ) {
Nurbol	0:d8528fbbaf9c	174	command->result_status = CMD_BAD_SERVO_VALUE;
Nurbol	0:d8528fbbaf9c	175	break;
Nurbol	0:d8528fbbaf9c	176	}
Nurbol	0:d8528fbbaf9c	177	if ((command->param[0] == 4) && (command->param[1] == 0)) {
Nurbol	0:d8528fbbaf9c	178	pulse_width = 0; // convert angle to pulse width
Nurbol	0:d8528fbbaf9c	179	}
Nurbol	0:d8528fbbaf9c	180	else{
Nurbol	0:d8528fbbaf9c	181	pulse_width = 1000 + (command->param[1] * 1000) / MAX_SERVO_ANGLE; // convert angle to pulse width
Nurbol	0:d8528fbbaf9c	182	}
Nurbol	0:d8528fbbaf9c	183
Nurbol	0:d8528fbbaf9c	184
Nurbol	0:d8528fbbaf9c	185
Nurbol	0:d8528fbbaf9c	186	//
Nurbol	0:d8528fbbaf9c	187	// implement servo move to all 5 servos
Nurbol	0:d8528fbbaf9c	188	//
Nurbol	0:d8528fbbaf9c	189	switch (command->param[0]) {
Nurbol	0:d8528fbbaf9c	190	case 0 : servo_0.pulsewidth_us(pulse_width); break;
Nurbol	0:d8528fbbaf9c	191	case 4 : servo_4.pulsewidth_us(pulse_width); break;
Nurbol	0:d8528fbbaf9c	192	case 5 : servo_5.pulsewidth_us(pulse_width); break;
Nurbol	0:d8528fbbaf9c	193
Nurbol	0:d8528fbbaf9c	194	}
Nurbol	0:d8528fbbaf9c	195	last_servo = command->param[0];
Nurbol	0:d8528fbbaf9c	196	break;
Nurbol	0:d8528fbbaf9c	197	//
Nurbol	0:d8528fbbaf9c	198	// return last servo number
Nurbol	0:d8528fbbaf9c	199	//
Nurbol	0:d8528fbbaf9c	200	case READ_CMD :
Nurbol	0:d8528fbbaf9c	201	command->nos_data = 2;// no data to be returned
Nurbol	0:d8528fbbaf9c	202	if((bSort_Mode == 0)&&(bMaint_Mode == 1)){
Nurbol	0:d8528fbbaf9c	203	command->result_data[0] = valueLED1;
Nurbol	0:d8528fbbaf9c	204	command->result_data[1] = valueLED2;
Nurbol	0:d8528fbbaf9c	205	}
Nurbol	0:d8528fbbaf9c	206	else if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
Nurbol	0:d8528fbbaf9c	207	command->result_data[0] = counter1p;
Nurbol	0:d8528fbbaf9c	208	command->result_data[1] = counter2p;
Nurbol	0:d8528fbbaf9c	209	}
Nurbol	0:d8528fbbaf9c	210	break;
Nurbol	0:d8528fbbaf9c	211
Nurbol	0:d8528fbbaf9c	212	//
Nurbol	0:d8528fbbaf9c	213	// Mode value
Nurbol	0:d8528fbbaf9c	214	//
Nurbol	0:d8528fbbaf9c	215	case MAINT_MODE :
Nurbol	0:d8528fbbaf9c	216	bSort_Mode = 0;
Nurbol	0:d8528fbbaf9c	217	bMaint_Mode = 1;
Nurbol	1:a91950a8e20f	218	servo_4.pulsewidth_us(0);
Nurbol	1:a91950a8e20f	219	servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);
Nurbol	1:a91950a8e20f	220	servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);
Nurbol	0:d8528fbbaf9c	221	break;
Nurbol	0:d8528fbbaf9c	222
Nurbol	0:d8528fbbaf9c	223
Nurbol	0:d8528fbbaf9c	224	case SORT_MODE :
Nurbol	0:d8528fbbaf9c	225	bSort_Mode = 1;
Nurbol	0:d8528fbbaf9c	226	bMaint_Mode = 0;
Nurbol	0:d8528fbbaf9c	227	break;
Nurbol	0:d8528fbbaf9c	228	//
Nurbol	0:d8528fbbaf9c	229	// catch any problems
Nurbol	0:d8528fbbaf9c	230	//
Nurbol	0:d8528fbbaf9c	231	default:
Nurbol	0:d8528fbbaf9c	232	command->nos_data = 0; // no data to be returned
Nurbol	0:d8528fbbaf9c	233	command->result_status = CMD_BAD_SERVO_VALUE;
Nurbol	0:d8528fbbaf9c	234	break;
Nurbol	0:d8528fbbaf9c	235	}
Nurbol	0:d8528fbbaf9c	236	return OK;
Nurbol	0:d8528fbbaf9c	237	}
Nurbol	0:d8528fbbaf9c	238
Nurbol	0:d8528fbbaf9c	239	//************************************************************************
Nurbol	0:d8528fbbaf9c	240
Nurbol	0:d8528fbbaf9c	241	//function to send value on the FPGA when 1p is detected
Nurbol	0:d8528fbbaf9c	242	void sensor1p (void){
Nurbol	0:d8528fbbaf9c	243	clk = !clk;
Nurbol	0:d8528fbbaf9c	244	wait(0.01);
Nurbol	0:d8528fbbaf9c	245	sensor1.read();
Nurbol	0:d8528fbbaf9c	246	if(sensor1 > 0.5) {
Nurbol	0:d8528fbbaf9c	247	led1 = 1;
Nurbol	1:a91950a8e20f	248	if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
Nurbol	1:a91950a8e20f	249	valueLED1 = 1;
Nurbol	1:a91950a8e20f	250	}
Nurbol	0:d8528fbbaf9c	251	}
Nurbol	0:d8528fbbaf9c	252	else if(sensor1 < 0.5){
Nurbol	0:d8528fbbaf9c	253	led1 = 0;
Nurbol	1:a91950a8e20f	254	if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
Nurbol	1:a91950a8e20f	255	valueLED1 = 0;
Nurbol	1:a91950a8e20f	256	}
Nurbol	0:d8528fbbaf9c	257	}
Nurbol	0:d8528fbbaf9c	258	}
Nurbol	0:d8528fbbaf9c	259
Nurbol	0:d8528fbbaf9c	260	//function to send value on the FPGA when 2p is detected
Nurbol	0:d8528fbbaf9c	261	void sensor2p (){
Nurbol	0:d8528fbbaf9c	262	sensor2.read();
Nurbol	0:d8528fbbaf9c	263	if(sensor2 > 0.5) {
Nurbol	0:d8528fbbaf9c	264	led2 = 1;
Nurbol	0:d8528fbbaf9c	265	valueLED2 = 1;
Nurbol	0:d8528fbbaf9c	266	}
Nurbol	0:d8528fbbaf9c	267	else if(sensor2 < 0.5){
Nurbol	0:d8528fbbaf9c	268	led2 = 0;
Nurbol	0:d8528fbbaf9c	269	valueLED2 = 0;
Nurbol	0:d8528fbbaf9c	270	}
Nurbol	0:d8528fbbaf9c	271	}
Nurbol	0:d8528fbbaf9c	272
Nurbol	0:d8528fbbaf9c	273
Nurbol	0:d8528fbbaf9c	274	//function for the state machine to move servos between 2 positions
Nurbol	0:d8528fbbaf9c	275	void state_machine (){
Nurbol	1:a91950a8e20f	276	if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
Nurbol	0:d8528fbbaf9c	277	switch(state)
Nurbol	0:d8528fbbaf9c	278	{
Nurbol	0:d8528fbbaf9c	279	case 0: // initial state
Nurbol	0:d8528fbbaf9c	280	servo_4.pulsewidth_us(1000 + (25 * 1000) / 90); // motor is run
Nurbol	0:d8528fbbaf9c	281	servo_0.pulsewidth_us(1000 + (0 * 1000) / 90); // servo 1p go to position 1
Nurbol	0:d8528fbbaf9c	282	servo_5.pulsewidth_us(1000 + (0 * 1000) / 90); // servo 2p go to position 1
Nurbol	0:d8528fbbaf9c	283	Position1_1p = 1;
Nurbol	0:d8528fbbaf9c	284	Position2_1p = 0;
Nurbol	0:d8528fbbaf9c	285	Position1_2p = 1;
Nurbol	0:d8528fbbaf9c	286	Position2_2p = 0;
Nurbol	0:d8528fbbaf9c	287	led3 = 0;
Nurbol	0:d8528fbbaf9c	288	led4 = 0;
Nurbol	0:d8528fbbaf9c	289	counter1p.read();
Nurbol	0:d8528fbbaf9c	290	counter2p.read();
Nurbol	0:d8528fbbaf9c	291	if(SW1p == 0){
Nurbol	0:d8528fbbaf9c	292	if(counter1p > 0.5){
Nurbol	0:d8528fbbaf9c	293	state = 1;
Nurbol	0:d8528fbbaf9c	294	}
Nurbol	0:d8528fbbaf9c	295	}
Nurbol	0:d8528fbbaf9c	296	if(SW2p == 0){
Nurbol	0:d8528fbbaf9c	297	if(counter2p > 0.5){
Nurbol	0:d8528fbbaf9c	298	state = 4;
Nurbol	0:d8528fbbaf9c	299	}
Nurbol	0:d8528fbbaf9c	300	}
Nurbol	0:d8528fbbaf9c	301	break;
Nurbol	0:d8528fbbaf9c	302	case 1: // state 1 if counter1p = 1
Nurbol	0:d8528fbbaf9c	303	servo_4.pulsewidth_us(0); // motor stop
Nurbol	0:d8528fbbaf9c	304	servo_0.pulsewidth_us(1000 + (200 * 1000) / 90); // servo 1p go to position 2
Nurbol	0:d8528fbbaf9c	305	wait(1);
Nurbol	0:d8528fbbaf9c	306	Position1_1p = 0;
Nurbol	0:d8528fbbaf9c	307	Position2_1p = 1;
Nurbol	0:d8528fbbaf9c	308	if((Position2_1p == 1)&&(counter1p < 0.5)){
Nurbol	0:d8528fbbaf9c	309	state = 2;
Nurbol	0:d8528fbbaf9c	310	}
Nurbol	0:d8528fbbaf9c	311	break;
Nurbol	0:d8528fbbaf9c	312	case 2: // state 2 if servo 1p is in position 2
Nurbol	0:d8528fbbaf9c	313	servo_4.pulsewidth_us(1000 + (25 * 1000) / 90); // motor is run
Nurbol	0:d8528fbbaf9c	314	counter1p.read();
Nurbol	0:d8528fbbaf9c	315	if(counter1p > 0.5){
Nurbol	0:d8528fbbaf9c	316	state = 3;
Nurbol	0:d8528fbbaf9c	317	}
Nurbol	0:d8528fbbaf9c	318	break;
Nurbol	0:d8528fbbaf9c	319	case 3: // state 3 if counter 1p = 1
Nurbol	0:d8528fbbaf9c	320	servo_4.pulsewidth_us(0); // motor stop
Nurbol	0:d8528fbbaf9c	321	led3 = 1;
Nurbol	0:d8528fbbaf9c	322	if(SW1p == 1){ // wait SW 1p to go to the initial state
Nurbol	0:d8528fbbaf9c	323	state = 0;
Nurbol	0:d8528fbbaf9c	324	}
Nurbol	0:d8528fbbaf9c	325	break;
Nurbol	0:d8528fbbaf9c	326	case 4: // state 4 if counter 2p = 1
Nurbol	0:d8528fbbaf9c	327	servo_4.pulsewidth_us(0); // motor stop
Nurbol	0:d8528fbbaf9c	328	servo_5.pulsewidth_us(1000 + (200 * 1000) / 90); // servo 2p go to position 2
Nurbol	0:d8528fbbaf9c	329	wait(1);
Nurbol	0:d8528fbbaf9c	330	Position1_2p = 0;
Nurbol	0:d8528fbbaf9c	331	Position2_2p = 1;
Nurbol	0:d8528fbbaf9c	332	if((Position2_2p == 1)&&(counter2p < 0.5)){
Nurbol	0:d8528fbbaf9c	333	state = 5;
Nurbol	0:d8528fbbaf9c	334	}
Nurbol	0:d8528fbbaf9c	335	break;
Nurbol	0:d8528fbbaf9c	336	case 5: // state 5 if servo 2p is in position 2
Nurbol	0:d8528fbbaf9c	337	servo_4.pulsewidth_us(1000 + (25 * 1000) / 90); // motor run
Nurbol	0:d8528fbbaf9c	338	counter2p.read();
Nurbol	0:d8528fbbaf9c	339	if(counter2p > 0.5){
Nurbol	0:d8528fbbaf9c	340	state = 6;
Nurbol	0:d8528fbbaf9c	341	}
Nurbol	0:d8528fbbaf9c	342	break;
Nurbol	0:d8528fbbaf9c	343	case 6: // state 6 if counter 2p = 1
Nurbol	0:d8528fbbaf9c	344	servo_4.pulsewidth_us(0); // motor stop
Nurbol	0:d8528fbbaf9c	345	led4 = 1;
Nurbol	0:d8528fbbaf9c	346	if(SW2p == 1){ // wait SW 2p to go to the initial state
Nurbol	0:d8528fbbaf9c	347	state = 0;
Nurbol	0:d8528fbbaf9c	348	}
Nurbol	0:d8528fbbaf9c	349	break;
Nurbol	1:a91950a8e20f	350	}
Nurbol	1:a91950a8e20f	351	}
Nurbol	0:d8528fbbaf9c	352	}
Nurbol	0:d8528fbbaf9c	353
Nurbol	0:d8528fbbaf9c	354
Nurbol	0:d8528fbbaf9c	355	//************************************************************************
Nurbol	0:d8528fbbaf9c	356	//
Nurbol	0:d8528fbbaf9c	357	int main() {
Nurbol	0:d8528fbbaf9c	358
Nurbol	0:d8528fbbaf9c	359	valueLED1=0;
Nurbol	0:d8528fbbaf9c	360	valueLED2=0;
Nurbol	0:d8528fbbaf9c	361	clk=0;
Nurbol	0:d8528fbbaf9c	362	init_sys();
Nurbol	0:d8528fbbaf9c	363
Nurbol	0:d8528fbbaf9c	364	FOREVER {
Nurbol	0:d8528fbbaf9c	365
Nurbol	0:d8528fbbaf9c	366	timer2p.attach(&sensor2p, 0.1); //function sensor2p is reading all the 0.1 ms
Nurbol	0:d8528fbbaf9c	367	timer1p.attach(&sensor1p, 0.1); //function sensor1p is reading all the 0.1 ms
Nurbol	0:d8528fbbaf9c	368
Nurbol	1:a91950a8e20f	369
Nurbol	0:d8528fbbaf9c	370	counter1p.read();
Nurbol	0:d8528fbbaf9c	371	counter2p.read();
Nurbol	0:d8528fbbaf9c	372
Nurbol	0:d8528fbbaf9c	373	timerstatemachine.attach(&state_machine, 0.1);
Nurbol	1:a91950a8e20f	374
Nurbol	0:d8528fbbaf9c	375
Nurbol	0:d8528fbbaf9c	376
Nurbol	0:d8528fbbaf9c	377	clk = !clk;
Nurbol	0:d8528fbbaf9c	378	wait(0.01);
Nurbol	0:d8528fbbaf9c	379
Nurbol	0:d8528fbbaf9c	380	get_cmd(&ext_cmd);
Nurbol	0:d8528fbbaf9c	381	//
Nurbol	0:d8528fbbaf9c	382	// Check status of read command activity and return an error status if there was a problem
Nurbol	0:d8528fbbaf9c	383	// If there is a problem, then return status code only and wait for next command.
Nurbol	0:d8528fbbaf9c	384	//
Nurbol	0:d8528fbbaf9c	385	if (ext_cmd.result_status != OK){
Nurbol	0:d8528fbbaf9c	386	send_status(ext_cmd.result_status);
Nurbol	0:d8528fbbaf9c	387	continue;
Nurbol	0:d8528fbbaf9c	388	}
Nurbol	0:d8528fbbaf9c	389	//
Nurbol	0:d8528fbbaf9c	390	// Parse command and return an error staus if there is a problem
Nurbol	0:d8528fbbaf9c	391	// If there is a problem, then return status code only and wait for next command.
Nurbol	0:d8528fbbaf9c	392	//
Nurbol	0:d8528fbbaf9c	393	parse_cmd(&ext_cmd);
Nurbol	0:d8528fbbaf9c	394	lcd->locate(1,0); lcd->printf(ext_cmd.cmd_str);
Nurbol	0:d8528fbbaf9c	395	if ((ext_cmd.result_status != OK) && (ext_cmd.cmd_code != TEXT_CMD)){
Nurbol	0:d8528fbbaf9c	396	lcd->locate(1,0); lcd->printf("parse : error");
Nurbol	0:d8528fbbaf9c	397	send_status(ext_cmd.result_status);
Nurbol	0:d8528fbbaf9c	398	continue;
Nurbol	0:d8528fbbaf9c	399	}
Nurbol	0:d8528fbbaf9c	400	//
Nurbol	0:d8528fbbaf9c	401	// Execute command and return an error staus if there is a problem
Nurbol	0:d8528fbbaf9c	402	//
Nurbol	0:d8528fbbaf9c	403	process_cmd(&ext_cmd);
Nurbol	0:d8528fbbaf9c	404	reply_to_cmd(&ext_cmd);
Nurbol	0:d8528fbbaf9c	405
Nurbol	0:d8528fbbaf9c	406
Nurbol	0:d8528fbbaf9c	407
Nurbol	0:d8528fbbaf9c	408	}
Nurbol	0:d8528fbbaf9c	409
Nurbol	0:d8528fbbaf9c	410	}
Nurbol	0:d8528fbbaf9c	411
Nurbol	0:d8528fbbaf9c	412
Nurbol	0:d8528fbbaf9c	413
Nurbol	0:d8528fbbaf9c	414
Nurbol	0:d8528fbbaf9c	415
Nurbol	0:d8528fbbaf9c	416
Nurbol	0:d8528fbbaf9c	417
Nurbol	0:d8528fbbaf9c	418
Nurbol	0:d8528fbbaf9c	419

Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	25 Nov 2011
Imports:	1
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	5
Followers:	2

Important changes to repositories hosted on mbed.com

main.cpp@1:a91950a8e20f, 2011-11-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

main.cpp@1:a91950a8e20f, 2011-11-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning