WebSVN – HomeAutomation – Diff – /trunk/EmbeddedSoftware/AVR/application/Servo/main.c



/**
 * CanServo.
 * Takes incomming packages and adjusts PWM output.
 * Input: 0 to 255
 * Position msg: <byte0 BLINDS_CMD> <byte1 position> <byte2 servo number>
 * Turning msg: <byte0 START/STOP> <byte 1 direction> <byte2 servo number>
 *
 * @date    2007-01-01
 * @author  Erik Larsson

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
/* lib files */
#include <bios.h>
#include <config.h>
#include <drivers/timer/timebase.h>
#include <tc1047.h>
#include <rc_servo.h>
 
#include <funcdefs.h>
 

#define BOUNCE_TIME             10 /* ms */
#define SERVO_FEED_TIME         2000 /* ms */
#define BUTTON1                 1
#define BUTTON2                 2
 
#define SERVO_FEED_PORT         PORTD
#define SERVO_FEED_DDR          DDRD
#define SERVO_FEED_PIN          PD2
 
 
 
 
/*-------------------------------------------------------------------------
 * Global variables
 * -----------------------------------------------------------------------*/
/*#if defined(TWO_BUTTON_MODE)
Can_Message_t txMsg_Button;
#endif*/
 
uint8_t msg_received = 0;
 
Can_Message_t txMsg;
Can_Message_t rxMsg;
 
/*----------------------------------------------------------------------------

        if(act_type == ACT_TYPE_SERVO){
            if(group == GROUP_ID || node == NODE_ID){
                for(m=0;m<msg->DataLength;m++){
                    rxMsg.Data.bytes[m] = msg->Data.bytes[m];
                }
            msg_received = 1;
            }
        }
    }
}
 
/*----------------------------------------------------------------------------
 * Interrupt routines
 * -------------------------------------------------------------------------*/
#if defined(TWO_BUTTON_MODE)
#error two button mode is still untested

    if( (PIND & (1<<PD1)) == 0){
        /* Button pressed */
        buttonTime[0] = Timebase_CurrentTime();
    }else if( (PIND & (1<<PD1)) == 1){
        if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
            /* No bounce, send message */
            txMsg_Button.Id = BUTTON_ID;
            txMsg_Button.DataLength = 1;
            txMsg_Button.Data.bytes[0] = BUTTON1;
            return;
        }else{
            /* Just bounces, ignore it */
            return;
        }
    }
    if( (PIND & (1<<PD2)) == 0){
        /* Button pressed */
        buttonTime[1] = Timebase_CurrentTime();
    }else if( (PIND & (1<<PD2)) == 1){

        }else{
            /* Just bounces, ignore it */
            return;
        }
    }
}
#endif
 
/*-----------------------------------------------------------------------------
 * Main Program
 *---------------------------------------------------------------------------*/
int main(viod) {
 
    uint8_t //blindsStatus, /* Position (-128 to 127) */
            servoToTurn,
            turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
    uint32_t boardTemperature = 0;
    uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
 
    sei();
   
    Timebase_Init();
    adcTemperatureInit();
    rcServoInit();
 
#if defined(TWO_BUTTON_MODE)
    /*

    txMsg.RemoteFlag = 0;
    txMsg.ExtendedFlag = 1;
    txMsg.Id = (CLASS_SNS << CLASS_MASK_BITS)|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)|(NODE_ID << SID_MASK_BITS);
 
    /* Set up callback for CAN messages */
    BIOS_CanCallback = &can_receive;
 
    /* main loop */
    while (1) {
        /* check if any messages have been received */
        if(msg_received){
            /* This node that control a servo is adressed */
            if( rxMsg.Data.bytes[0] == BLINDS_CMD_ABS ){
                /* Set absolute position to servo */
                setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
                servoFeed_timeStamp = Timebase_CurrentTime();
 
            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){
                /* Set relative position to servo */
                alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
                servoFeed_timeStamp = Timebase_CurrentTime();
 
            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){
                /* Start turning the servo */
                turnDirection = rxMsg.Data.bytes[1];
                servoToTurn = rxMsg.Data.bytes[2];
                servoFeed_timeStamp = Timebase_CurrentTime();
 
            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){
                /* Stop turning */
                turnDirection = BLINDS_TURN_STOP;
 

#endif
#if NUMBER_OF_RCS >2
                    txMsg.Data.bytes[5] = getPosition(3);
#endif
                    txMsg.DataLength = NUMBER_OF_RCS+3;
                    BIOS_CanSend(&txMsg);
                    }
                }
                msg_received = 0;
        }
 
        if( (Timebase_CurrentTime() - timeStamp)  >= STATUS_SEND_PERIOD ){
            timeStamp = Timebase_CurrentTime();
            /* Send blinds status to CAN */
 
            boardTemperature = getTC1047temperature();
 
            if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){

#if NUMBER_OF_RCS >2
                txMsg.Data.bytes[5] = getPosition(3);
#endif
                txMsg.DataLength = NUMBER_OF_RCS+3;
 
                BIOS_CanSend(&txMsg);
            }
        }
        /* If start turning servo */
        if( turnDirection ){
            if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
                timeStampTurn = Timebase_CurrentTime();
                /* Clockwise or counterclockwise? */
                if( turnDirection == BLINDS_TURN_LEFT ){
                    alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);
                }else if( turnDirection == BLINDS_TURN_RIGHT ){
                    alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);
                }
            }
        }
        /* Turn off the servo current feed */
        if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
            SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
        }else{
            SERVO_FEED_PORT |= (1<<SERVO_FEED_PIN);
        }
    }
    return 0;
}
 
void blindsFailsafe()
{
 
    uint32_t boardTemperature = 0, timeStamp = 0;
 
    /* Turn of servo current feed */
    SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
 

                /* Return from failsafe mode */
                return;
            }
        }
 
        if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
            timeStamp = Timebase_CurrentTime();
            /* Send relay status to CAN */
            boardTemperature = getTC1047temperature();
 
            txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
            txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;

#if NUMBER_OF_RCS >2
            txMsg.Data.bytes[5] = getPosition(3);
#endif
            txMsg.DataLength = NUMBER_OF_RCS+3;
 
            BIOS_CanSend(&txMsg);
        }
    }
}
 

Rev 387	Rev 388
Line 1...	Line 1...
1	/**	1	/**
2	* ~~CanBlinds~~.	2	* CanServo.
3	* Takes incomming packages and adjusts PWM output.	3	* Takes incomming packages and adjusts PWM output.
4	* Input: ~~-128~~ to ~~+127~~	4	* Input: 0 to 255
5	* Position msg: <byte0 BLINDS_CMD> <byte1 position> <byte2 servo number>	5	* Position msg: <byte0 BLINDS_CMD> <byte1 position> <byte2 servo number>
6	* Turning msg: <byte0 START/STOP> <byte 1 direction> <byte2 servo number>	6	* Turning msg: <byte0 START/STOP> <byte 1 direction> <byte2 servo number>
7	*	7	*
8	* @date 2007-01-01	8	* @date 2007-01-01
9	* @author Erik Larsson	9	* @author Erik Larsson
Line 19...	Line 19...
19	#include <avr/io.h>	19	#include <avr/io.h>
20	#include <avr/interrupt.h>	20	#include <avr/interrupt.h>
21	#include <stdio.h>	21	#include <stdio.h>
22	/* lib files */	22	/* lib files */
23	#include <bios.h>	23	#include <bios.h>
-		24	#include <config.h>
-		25	#include <drivers/timer/timebase.h>
24	#include <tc1047.h>	26	#include <tc1047.h>
25	#include <rc_servo.h>	27	#include <rc_servo.h>
26		28
27	#include <funcdefs.h>	29	#include <funcdefs.h>
28		30
Line 50...	Line 52...
50	#define BOUNCE_TIME 10 /* ms */	52	#define BOUNCE_TIME 10 /* ms */
51	#define SERVO_FEED_TIME 2000 /* ms */	53	#define SERVO_FEED_TIME 2000 /* ms */
52	#define BUTTON1 1	54	#define BUTTON1 1
53	#define BUTTON2 2	55	#define BUTTON2 2
54		56
55	#define SERVO_FEED_PORT ~~PORTC~~	57	#define SERVO_FEED_PORT PORTD
56	#define SERVO_FEED_DDR ~~DDRC~~	58	#define SERVO_FEED_DDR DDRD
57	#define SERVO_FEED_PIN ~~PC0~~	59	#define SERVO_FEED_PIN PD2
58		60
59	#define TRUE 1	-
60	#define FALSE !TRUE	-
61		61
62	/*-------------------------------------------------------------------------	62	/*-------------------------------------------------------------------------
63	* Global variables	63	* Global variables
64	* -----------------------------------------------------------------------*/	64	* -----------------------------------------------------------------------*/
65	/*#if defined(TWO_BUTTON_MODE)	65	/*#if defined(TWO_BUTTON_MODE)
66	Can_Message_t txMsg_Button;	66	Can_Message_t txMsg_Button;
67	#endif*/	67	#endif*/
68		68
69	uint8_t msg_received = ~~FALSE~~;	69	uint8_t msg_received = 0;
70		70
71	Can_Message_t txMsg;	71	Can_Message_t txMsg;
72	Can_Message_t rxMsg;	72	Can_Message_t rxMsg;
73		73
74	/*----------------------------------------------------------------------------	74	/*----------------------------------------------------------------------------
Line 90...	Line 90...
90	if(act_type == ACT_TYPE_SERVO){	90	if(act_type == ACT_TYPE_SERVO){
91	if(group == GROUP_ID \|\| node == NODE_ID){	91	if(group == GROUP_ID \|\| node == NODE_ID){
92	for(m=0;m<msg->DataLength;m++){	92	for(m=0;m<msg->DataLength;m++){
93	rxMsg.Data.bytes[m] = msg->Data.bytes[m];	93	rxMsg.Data.bytes[m] = msg->Data.bytes[m];
94	}	94	}
95	msg_received = ~~TRUE~~;	95	msg_received = 1;
96	}	96	}
97	}	97	}
98	}	98	}
99	}	99	}
100		-
101	/*----------------------------------------------------------------------------	100	/*----------------------------------------------------------------------------
102	* Interrupt routines	101	* Interrupt routines
103	* -------------------------------------------------------------------------*/	102	* -------------------------------------------------------------------------*/
104	#if defined(TWO_BUTTON_MODE)	103	#if defined(TWO_BUTTON_MODE)
105	#error two button mode is still untested	104	#error two button mode is still untested
Line 117...	Line 116...
117	if( (PIND & (1<<PD1)) == 0){	116	if( (PIND & (1<<PD1)) == 0){
118	/* Button pressed */	117	/* Button pressed */
119	buttonTime[0] = Timebase_CurrentTime();	118	buttonTime[0] = Timebase_CurrentTime();
120	}else if( (PIND & (1<<PD1)) == 1){	119	}else if( (PIND & (1<<PD1)) == 1){
121	if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){	120	if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
122	/* No bounce, send message */	121	/* No bounce, send message */
123	txMsg_Button.Id = BUTTON_ID;	122	txMsg_Button.Id = BUTTON_ID;
124	txMsg_Button.DataLength = 1;	123	txMsg_Button.DataLength = 1;
125	txMsg_Button.Data.bytes[0] = BUTTON1;	124	txMsg_Button.Data.bytes[0] = BUTTON1;
126	return;	125	return;
127	}else{	126	}else{
128	/* Just bounces, ignore it */	127	/* Just bounces, ignore it */
129	return;	128	return;
130	}	129	}
131	}	130	}
132	if( (PIND & (1<<PD2)) == 0){	131	if( (PIND & (1<<PD2)) == 0){
133	/* Button pressed */	132	/* Button pressed */
134	buttonTime[1] = Timebase_CurrentTime();	133	buttonTime[1] = Timebase_CurrentTime();
135	}else if( (PIND & (1<<PD2)) == 1){	134	}else if( (PIND & (1<<PD2)) == 1){
Line 142...	Line 141...
142	}else{	141	}else{
143	/* Just bounces, ignore it */	142	/* Just bounces, ignore it */
144	return;	143	return;
145	}	144	}
146	}	145	}
147	}	146	}
148	#endif	147	#endif
149		148
150	/*-----------------------------------------------------------------------------	149	/*-----------------------------------------------------------------------------
151	* Main Program	150	* Main Program
152	---------------------------------------------------------------------------/	151	---------------------------------------------------------------------------/
153	int main(~~void~~) {	152	int main(viod) {
154		153
155	uint8_t //blindsStatus, /* Position (-128 to 127) */	154	uint8_t //blindsStatus, /* Position (-128 to 127) */
156	servoToTurn,	155	servoToTurn,
157	turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */	156	turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
158	uint32_t boardTemperature = 0;	157	uint32_t boardTemperature = 0;
159	uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;	158	uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
160		159
161	sei();	160	sei();
-		161
-		162	Timebase_Init();
162	adcTemperatureInit();	163	adcTemperatureInit();
163	rcServoInit();	164	rcServoInit();
164		165
165	#if defined(TWO_BUTTON_MODE)	166	#if defined(TWO_BUTTON_MODE)
166	/*	167	/*
Line 184...	Line 185...
184	txMsg.RemoteFlag = 0;	185	txMsg.RemoteFlag = 0;
185	txMsg.ExtendedFlag = 1;	186	txMsg.ExtendedFlag = 1;
186	txMsg.Id = (CLASS_SNS << CLASS_MASK_BITS)\|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)\|(NODE_ID << SID_MASK_BITS);	187	txMsg.Id = (CLASS_SNS << CLASS_MASK_BITS)\|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)\|(NODE_ID << SID_MASK_BITS);
187		188
188	/* Set up callback for CAN messages */	189	/* Set up callback for CAN messages */
189	~~bios->can_callback~~ = &can_receive;	190	BIOS_CanCallback = &can_receive;
190		191
191	/* main loop */	192	/* main loop */
192	while (1) {	193	while (1) {
193	/* check if any messages have been received */	194	/* check if any messages have been received */
194	if(msg_received){	195	if(msg_received){
195	/* This node that control a servo is adressed */	196	/* This node that control a servo is adressed */
196	if( rxMsg.Data.bytes[0] == BLINDS_CMD_ABS ){	197	if( rxMsg.Data.bytes[0] == BLINDS_CMD_ABS ){
197	/* Set absolute position to servo */	198	/* Set absolute position to servo */
198	setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );	199	setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
199	servoFeed_timeStamp = ~~bios->timebase_get~~();	200	servoFeed_timeStamp = Timebase_CurrentTime();
200		201
201	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){	202	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){
202	/* Set relative position to servo */	203	/* Set relative position to servo */
203	alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );	204	alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
204	servoFeed_timeStamp = ~~bios->timebase_get~~();	205	servoFeed_timeStamp = Timebase_CurrentTime();
205		206
206	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){	207	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){
207	/* Start turning the servo */	208	/* Start turning the servo */
208	turnDirection = rxMsg.Data.bytes[1];	209	turnDirection = rxMsg.Data.bytes[1];
209	servoToTurn = rxMsg.Data.bytes[2];	210	servoToTurn = rxMsg.Data.bytes[2];
210	servoFeed_timeStamp = ~~bios->timebase_get~~();	211	servoFeed_timeStamp = Timebase_CurrentTime();
211		212
212	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){	213	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){
213	/* Stop turning */	214	/* Stop turning */
214	turnDirection = BLINDS_TURN_STOP;	215	turnDirection = BLINDS_TURN_STOP;
215		216
Line 231...	Line 232...
231	#endif	232	#endif
232	#if NUMBER_OF_RCS >2	233	#if NUMBER_OF_RCS >2
233	txMsg.Data.bytes[5] = getPosition(3);	234	txMsg.Data.bytes[5] = getPosition(3);
234	#endif	235	#endif
235	txMsg.DataLength = NUMBER_OF_RCS+3;	236	txMsg.DataLength = NUMBER_OF_RCS+3;
236	~~bios->can_send~~(&txMsg);	237	BIOS_CanSend(&txMsg);
237	}	238	}
238	}	239	}
239	msg_received = ~~FALSE~~;	240	msg_received = 0;
240	}	241	}
241		242
242	if( (~~bios->timebase_get~~() - timeStamp) >= STATUS_SEND_PERIOD ){	243	if( (Timebase_CurrentTime() - timeStamp) >= STATUS_SEND_PERIOD ){
243	timeStamp = ~~bios->timebase_get~~();	244	timeStamp = Timebase_CurrentTime();
244	/* Send blinds status to CAN */	245	/* Send blinds status to CAN */
245		246
246	boardTemperature = getTC1047temperature();	247	boardTemperature = getTC1047temperature();
247		248
248	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){	249	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
Line 260...	Line 261...
260	#if NUMBER_OF_RCS >2	261	#if NUMBER_OF_RCS >2
261	txMsg.Data.bytes[5] = getPosition(3);	262	txMsg.Data.bytes[5] = getPosition(3);
262	#endif	263	#endif
263	txMsg.DataLength = NUMBER_OF_RCS+3;	264	txMsg.DataLength = NUMBER_OF_RCS+3;
264		265
265	~~bios->can_send~~(&txMsg);	266	BIOS_CanSend(&txMsg);
266	}	267	}
267	}	268	}
268	/* If start turning servo */	269	/* If start turning servo */
269	if( turnDirection ){	270	if( turnDirection ){
270	if( ~~bios->timebase_get~~() - timeStampTurn >= TURN_PERIOD ){	271	if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
271	timeStampTurn = ~~bios->timebase_get~~();	272	timeStampTurn = Timebase_CurrentTime();
272	/* Clockwise or counterclockwise? */	273	/* Clockwise or counterclockwise? */
273	if( turnDirection == BLINDS_TURN_LEFT ){	274	if( turnDirection == BLINDS_TURN_LEFT ){
274	alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);	275	alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);
275	}else if( turnDirection == BLINDS_TURN_RIGHT ){	276	}else if( turnDirection == BLINDS_TURN_RIGHT ){
276	alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);	277	alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);
277	}	278	}
278	}	279	}
279	}	280	}
280	/* Turn off the servo current feed */	281	/* Turn off the servo current feed */
281	if( ~~bios->timebase_get~~() - servoFeed_timeStamp >= SERVO_FEED_TIME ){	282	if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
282	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);	283	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
283	}else{	284	}else{
284	SERVO_FEED_PORT \|= (1<<SERVO_FEED_PIN);	285	SERVO_FEED_PORT \|= (1<<SERVO_FEED_PIN);
285	}	286	}
286	}	287	}
287	return 0;	288	return 0;
288	}	289	}
289		290
290	void blindsFailsafe()	291	void blindsFailsafe()
291	{	292	{
292	uint8_t relayStatus;	-
293	uint32_t boardTemperature = 0, timeStamp = 0;	293	uint32_t boardTemperature = 0, timeStamp = 0;
294		294
295	/* Turn of servo current feed */	295	/* Turn of servo current feed */
296	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);	296	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
297		297
Line 304...	Line 304...
304	/* Return from failsafe mode */	304	/* Return from failsafe mode */
305	return;	305	return;
306	}	306	}
307	}	307	}
308		308
309	if( ~~bios->timebase_get~~() - timeStamp >= FAILSAFE_SEND_PERIOD ){	309	if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
310	timeStamp = ~~bios->timebase_get~~();	310	timeStamp = Timebase_CurrentTime();
311	/* Send relay status to CAN */	311	/* Send relay status to CAN */
312	boardTemperature = getTC1047temperature();	312	boardTemperature = getTC1047temperature();
313		313
314	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;	314	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
315	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;	315	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
Line 323...	Line 323...
323	#if NUMBER_OF_RCS >2	323	#if NUMBER_OF_RCS >2
324	txMsg.Data.bytes[5] = getPosition(3);	324	txMsg.Data.bytes[5] = getPosition(3);
325	#endif	325	#endif
326	txMsg.DataLength = NUMBER_OF_RCS+3;	326	txMsg.DataLength = NUMBER_OF_RCS+3;
327		327
328	~~bios->can_send~~(&txMsg);	328	BIOS_CanSend(&txMsg);
329	}	329	}
330	}	330	}
331	}	331	}
332		332

Subversion Repositories HomeAutomation

(root)/trunk/EmbeddedSoftware/AVR/application/Servo/main.c @ 1663 – Rev 387 → 388