WebSVN – HomeAutomation – Diff – /trunk/EmbeddedSoftware/AVR/application/CanServo/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
 *
 */
 
//#define TWO_BUTTON_MODE /* If using two (or just one but nothing more) auxiliary buttons */
 
/*-----------------------------------------------------------------------------
 * Includes
 *---------------------------------------------------------------------------*/
/* system files */
#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>
 
/* defines */
#define GROUP_ID    0x01L // FIXME
 
#define FAILSAFE_MODE       0x0F
#define FAILSAFE_TRESHOLD   60  /* Degrees celcius when nodeBlinds goes into failsafe mode */
 
#define BLINDS_CMD_ABS      0x01 /* Absolute position */
#define BLINDS_CMD_REL      0x02 /* Relative position */
#define BLINDS_CMD_START    0x03 /* Start turning RC-servo */
#define BLINDS_CMD_STOP     0x04 /* Stop turning RC-servo */
#define BLINDS_CMD_RESET    0x05
#define BLINDS_CMD_STATUS   0x06
 
#define BLINDS_TURN_STOP    0x00
#define BLINDS_TURN_LEFT    0x01
#define BLINDS_TURN_RIGHT   0x02
 
#define STEPS_PER_TURN_PERIOD   10 /* number of steps for each adjustment */
#define TURN_PERIOD             100 /* ms */
#define STATUS_SEND_PERIOD      5000 /* ms */
#define FAILSAFE_SEND_PERIOD    10000 /* ms */
#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;
 
/*----------------------------------------------------------------------------
 * Functions
 * -------------------------------------------------------------------------*/
void blindsFailsafe();
 
/* Takes care about incoming messages and parses them if this node is adressed */
void can_receive(Can_Message_t *msg){
    uint32_t act;
    uint8_t m, act_type, group, node;
    if( ((msg->Id & CLASS_MASK) >> CLASS_MASK_BITS) == CLASS_ACT ){
        act = (msg->Id & TYPE_MASK) >> TYPE_MASK_BITS;
        act_type = (act & ACT_TYPE_MASK) >> ACT_TYPE_BITS;
        group = (act & ACT_GROUP_MASK) >> ACT_GROUP_BITS;
        node = (act & ACT_NODE_MASK) >> ACT_NODE_BITS;
 
        /* Check if it is command to control this servo */
        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
ISR( PCINT2_vect )
{
    /*
     * Auxiliary button pressed and released
     * Check time between press and release to eliminate bounces
     */
    static uint32_t buttonTime[2];
 
    txMsg_Button.RemoteFlag = 0;
    txMsg_Button.ExtendedFlag = 1;
 
    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){
        if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){
            /* No bounce, send message */
            txMsg_Button.Id = BUTTON_ID;
            txMsg_Button.DataLength = 1;
            txMsg_Button.Data.bytes[0] = BUTTON2;
            return;
        }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)
    /*
     * Initialize buttons
     * It is possible to use ie. sensors instead
     * but then change this
     */
    /* Set as input and enable pull-up */
    DDRD &= ~( (1<<DDD1)|(1<<DDD2) );
    PORTD |= (1<<PD1)|(1<<PD2);
 
    /* Enable IO-pin interrupt */
    PCICR |= (1<<PCIE2);
    /* Unmask PD1 and SERVO_FEED_PIN */
    PCMSK2 |= (1<<PCINT16) | (1<<PCINT17);
#endif
    /* Set output and turn off the servo current feed */
    SERVO_FEED_DDR |= (1<<SERVO_FEED_PIN);
    SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
 
    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;
 
            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
                /* Send blinds status to CAN */
                boardTemperature = getTC1047temperature();
 
                if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
                    blindsFailsafe();
                }else{
                    txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
                    txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
                    txMsg.Data.bytes[2] = 0x00;
#if NUMBER_OF_RCS >0
                    txMsg.Data.bytes[3] = getPosition(1);
#endif
#if NUMBER_OF_RCS >1
                    txMsg.Data.bytes[4] = getPosition(2);
#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 ){
                blindsFailsafe();
            }else{
                txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
                txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
                txMsg.Data.bytes[2] = 0x00;
#if NUMBER_OF_RCS >0
                txMsg.Data.bytes[3] = getPosition(1);
#endif
#if NUMBER_OF_RCS >1
                txMsg.Data.bytes[4] = getPosition(2);
#endif
#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);
 
 
    while(1){
        /* check if any messages have been received */
        while(msg_received) {
            /* This relay is adressed*/
            if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){
                /* 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;
            txMsg.Data.bytes[2] = FAILSAFE_MODE;
#if NUMBER_OF_RCS >0
            txMsg.Data.bytes[3] = getPosition(1);
#endif
#if NUMBER_OF_RCS >1
            txMsg.Data.bytes[4] = getPosition(2);
#endif
#if NUMBER_OF_RCS >2
            txMsg.Data.bytes[5] = getPosition(3);
#endif
            txMsg.DataLength = NUMBER_OF_RCS+3;
 
            BIOS_CanSend(&txMsg);
        }
    }
}
 
 

Rev 388	Rev 392
1	/**	1	/**
2	* CanServo.	2	* CanServo.
3	* Takes incomming packages and adjusts PWM output.	3	* Takes incomming packages and adjusts PWM output.
4	* Input: 0 to 255	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
10	*	10	*
11	*/	11	*/
12		12
13	//#define TWO_BUTTON_MODE /* If using two (or just one but nothing more) auxiliary buttons */	13	//#define TWO_BUTTON_MODE /* If using two (or just one but nothing more) auxiliary buttons */
14		14
15	/*-----------------------------------------------------------------------------	15	/*-----------------------------------------------------------------------------
16	* Includes	16	* Includes
17	---------------------------------------------------------------------------/	17	---------------------------------------------------------------------------/
18	/* system files */	18	/* system files */
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>	24	#include <config.h>
25	#include <drivers/timer/timebase.h>	25	#include <drivers/timer/timebase.h>
26	#include <tc1047.h>	26	#include <tc1047.h>
27	#include <rc_servo.h>	27	#include <rc_servo.h>
28		28
29	#include <funcdefs.h>	29	#include <funcdefs.h>
30		30
31	/* defines */	31	/* defines */
32	#define GROUP_ID 0x01L // FIXME	32	#define GROUP_ID 0x01L // FIXME
33		33
34	#define FAILSAFE_MODE 0x0F	34	#define FAILSAFE_MODE 0x0F
35	#define FAILSAFE_TRESHOLD 60 /* Degrees celcius when nodeBlinds goes into failsafe mode */	35	#define FAILSAFE_TRESHOLD 60 /* Degrees celcius when nodeBlinds goes into failsafe mode */
36		36
37	#define BLINDS_CMD_ABS 0x01 /* Absolute position */	37	#define BLINDS_CMD_ABS 0x01 /* Absolute position */
38	#define BLINDS_CMD_REL 0x02 /* Relative position */	38	#define BLINDS_CMD_REL 0x02 /* Relative position */
39	#define BLINDS_CMD_START 0x03 /* Start turning RC-servo */	39	#define BLINDS_CMD_START 0x03 /* Start turning RC-servo */
40	#define BLINDS_CMD_STOP 0x04 /* Stop turning RC-servo */	40	#define BLINDS_CMD_STOP 0x04 /* Stop turning RC-servo */
41	#define BLINDS_CMD_RESET 0x05	41	#define BLINDS_CMD_RESET 0x05
42	#define BLINDS_CMD_STATUS 0x06	42	#define BLINDS_CMD_STATUS 0x06
43		43
44	#define BLINDS_TURN_STOP 0x00	44	#define BLINDS_TURN_STOP 0x00
45	#define BLINDS_TURN_LEFT 0x01	45	#define BLINDS_TURN_LEFT 0x01
46	#define BLINDS_TURN_RIGHT 0x02	46	#define BLINDS_TURN_RIGHT 0x02
47		47
48	#define STEPS_PER_TURN_PERIOD 10 /* number of steps for each adjustment */	48	#define STEPS_PER_TURN_PERIOD 10 /* number of steps for each adjustment */
49	#define TURN_PERIOD 100 /* ms */	49	#define TURN_PERIOD 100 /* ms */
50	#define STATUS_SEND_PERIOD 5000 /* ms */	50	#define STATUS_SEND_PERIOD 5000 /* ms */
51	#define FAILSAFE_SEND_PERIOD 10000 /* ms */	51	#define FAILSAFE_SEND_PERIOD 10000 /* ms */
52	#define BOUNCE_TIME 10 /* ms */	52	#define BOUNCE_TIME 10 /* ms */
53	#define SERVO_FEED_TIME 2000 /* ms */	53	#define SERVO_FEED_TIME 2000 /* ms */
54	#define BUTTON1 1	54	#define BUTTON1 1
55	#define BUTTON2 2	55	#define BUTTON2 2
56		56
57	#define SERVO_FEED_PORT PORTD	57	#define SERVO_FEED_PORT PORTD
58	#define SERVO_FEED_DDR DDRD	58	#define SERVO_FEED_DDR DDRD
59	#define SERVO_FEED_PIN PD2	59	#define SERVO_FEED_PIN PD2
60		60
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 = 0;	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	/*----------------------------------------------------------------------------
75	* Functions	75	* Functions
76	* -------------------------------------------------------------------------*/	76	* -------------------------------------------------------------------------*/
77	void blindsFailsafe();	77	void blindsFailsafe();
78		78
79	/* Takes care about incoming messages and parses them if this node is adressed */	79	/* Takes care about incoming messages and parses them if this node is adressed */
80	void can_receive(Can_Message_t *msg){	80	void can_receive(Can_Message_t *msg){
81	uint32_t act;	81	uint32_t act;
82	uint8_t m, act_type, group, node;	82	uint8_t m, act_type, group, node;
83	if( ((msg->Id & CLASS_MASK) >> CLASS_MASK_BITS) == CLASS_ACT ){	83	if( ((msg->Id & CLASS_MASK) >> CLASS_MASK_BITS) == CLASS_ACT ){
84	act = (msg->Id & TYPE_MASK) >> TYPE_MASK_BITS;	84	act = (msg->Id & TYPE_MASK) >> TYPE_MASK_BITS;
85	act_type = (act & ACT_TYPE_MASK) >> ACT_TYPE_BITS;	85	act_type = (act & ACT_TYPE_MASK) >> ACT_TYPE_BITS;
86	group = (act & ACT_GROUP_MASK) >> ACT_GROUP_BITS;	86	group = (act & ACT_GROUP_MASK) >> ACT_GROUP_BITS;
87	node = (act & ACT_NODE_MASK) >> ACT_NODE_BITS;	87	node = (act & ACT_NODE_MASK) >> ACT_NODE_BITS;
88		88
89	/* Check if it is command to control this servo */	89	/* Check if it is command to control this servo */
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 = 1;	95	msg_received = 1;
96	}	96	}
97	}	97	}
98	}	98	}
99	}	99	}
100	/*----------------------------------------------------------------------------	100	/*----------------------------------------------------------------------------
101	* Interrupt routines	101	* Interrupt routines
102	* -------------------------------------------------------------------------*/	102	* -------------------------------------------------------------------------*/
103	#if defined(TWO_BUTTON_MODE)	103	#if defined(TWO_BUTTON_MODE)
104	#error two button mode is still untested	104	#error two button mode is still untested
105	ISR( PCINT2_vect )	105	ISR( PCINT2_vect )
106	{	106	{
107	/*	107	/*
108	* Auxiliary button pressed and released	108	* Auxiliary button pressed and released
109	* Check time between press and release to eliminate bounces	109	* Check time between press and release to eliminate bounces
110	*/	110	*/
111	static uint32_t buttonTime[2];	111	static uint32_t buttonTime[2];
112		112
113	txMsg_Button.RemoteFlag = 0;	113	txMsg_Button.RemoteFlag = 0;
114	txMsg_Button.ExtendedFlag = 1;	114	txMsg_Button.ExtendedFlag = 1;
115		115
116	if( (PIND & (1<<PD1)) == 0){	116	if( (PIND & (1<<PD1)) == 0){
117	/* Button pressed */	117	/* Button pressed */
118	buttonTime[0] = Timebase_CurrentTime();	118	buttonTime[0] = Timebase_CurrentTime();
119	}else if( (PIND & (1<<PD1)) == 1){	119	}else if( (PIND & (1<<PD1)) == 1){
120	if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){	120	if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
121	/* No bounce, send message */	121	/* No bounce, send message */
122	txMsg_Button.Id = BUTTON_ID;	122	txMsg_Button.Id = BUTTON_ID;
123	txMsg_Button.DataLength = 1;	123	txMsg_Button.DataLength = 1;
124	txMsg_Button.Data.bytes[0] = BUTTON1;	124	txMsg_Button.Data.bytes[0] = BUTTON1;
125	return;	125	return;
126	}else{	126	}else{
127	/* Just bounces, ignore it */	127	/* Just bounces, ignore it */
128	return;	128	return;
129	}	129	}
130	}	130	}
131	if( (PIND & (1<<PD2)) == 0){	131	if( (PIND & (1<<PD2)) == 0){
132	/* Button pressed */	132	/* Button pressed */
133	buttonTime[1] = Timebase_CurrentTime();	133	buttonTime[1] = Timebase_CurrentTime();
134	}else if( (PIND & (1<<PD2)) == 1){	134	}else if( (PIND & (1<<PD2)) == 1){
135	if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){	135	if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){
136	/* No bounce, send message */	136	/* No bounce, send message */
137	txMsg_Button.Id = BUTTON_ID;	137	txMsg_Button.Id = BUTTON_ID;
138	txMsg_Button.DataLength = 1;	138	txMsg_Button.DataLength = 1;
139	txMsg_Button.Data.bytes[0] = BUTTON2;	139	txMsg_Button.Data.bytes[0] = BUTTON2;
140	return;	140	return;
141	}else{	141	}else{
142	/* Just bounces, ignore it */	142	/* Just bounces, ignore it */
143	return;	143	return;
144	}	144	}
145	}	145	}
146	}	146	}
147	#endif	147	#endif
148		148
149	/*-----------------------------------------------------------------------------	149	/*-----------------------------------------------------------------------------
150	* Main Program	150	* Main Program
151	---------------------------------------------------------------------------/	151	---------------------------------------------------------------------------/
152	int main(viod) {	152	int main(viod) {
153		153
154	uint8_t //blindsStatus, /* Position (-128 to 127) */	154	uint8_t //blindsStatus, /* Position (-128 to 127) */
155	servoToTurn,	155	servoToTurn,
156	turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */	156	turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
157	uint32_t boardTemperature = 0;	157	uint32_t boardTemperature = 0;
158	uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;	158	uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
159		159
160	sei();	160	sei();
161		161
162	Timebase_Init();	162	Timebase_Init();
163	adcTemperatureInit();	163	adcTemperatureInit();
164	rcServoInit();	164	rcServoInit();
165		165
166	#if defined(TWO_BUTTON_MODE)	166	#if defined(TWO_BUTTON_MODE)
167	/*	167	/*
168	* Initialize buttons	168	* Initialize buttons
169	* It is possible to use ie. sensors instead	169	* It is possible to use ie. sensors instead
170	* but then change this	170	* but then change this
171	*/	171	*/
172	/* Set as input and enable pull-up */	172	/* Set as input and enable pull-up */
173	DDRD &= ~( (1<<DDD1)\|(1<<DDD2) );	173	DDRD &= ~( (1<<DDD1)\|(1<<DDD2) );
174	PORTD \|= (1<<PD1)\|(1<<PD2);	174	PORTD \|= (1<<PD1)\|(1<<PD2);
175		175
176	/* Enable IO-pin interrupt */	176	/* Enable IO-pin interrupt */
177	PCICR \|= (1<<PCIE2);	177	PCICR \|= (1<<PCIE2);
178	/* Unmask PD1 and SERVO_FEED_PIN */	178	/* Unmask PD1 and SERVO_FEED_PIN */
179	PCMSK2 \|= (1<<PCINT16) \| (1<<PCINT17);	179	PCMSK2 \|= (1<<PCINT16) \| (1<<PCINT17);
180	#endif	180	#endif
181	/* Set output and turn off the servo current feed */	181	/* Set output and turn off the servo current feed */
182	SERVO_FEED_DDR \|= (1<<SERVO_FEED_PIN);	182	SERVO_FEED_DDR \|= (1<<SERVO_FEED_PIN);
183	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);	183	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
184		184
185	txMsg.RemoteFlag = 0;	185	txMsg.RemoteFlag = 0;
186	txMsg.ExtendedFlag = 1;	186	txMsg.ExtendedFlag = 1;
187	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);
188		188
189	/* Set up callback for CAN messages */	189	/* Set up callback for CAN messages */
190	BIOS_CanCallback = &can_receive;	190	BIOS_CanCallback = &can_receive;
191		191
192	/* main loop */	192	/* main loop */
193	while (1) {	193	while (1) {
194	/* check if any messages have been received */	194	/* check if any messages have been received */
195	if(msg_received){	195	if(msg_received){
196	/* This node that control a servo is adressed */	196	/* This node that control a servo is adressed */
197	if( rxMsg.Data.bytes[0] == BLINDS_CMD_ABS ){	197	if( rxMsg.Data.bytes[0] == BLINDS_CMD_ABS ){
198	/* Set absolute position to servo */	198	/* Set absolute position to servo */
199	setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );	199	setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
200	servoFeed_timeStamp = Timebase_CurrentTime();	200	servoFeed_timeStamp = Timebase_CurrentTime();
201		201
202	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){	202	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){
203	/* Set relative position to servo */	203	/* Set relative position to servo */
204	alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );	204	alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
205	servoFeed_timeStamp = Timebase_CurrentTime();	205	servoFeed_timeStamp = Timebase_CurrentTime();
206		206
207	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){	207	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){
208	/* Start turning the servo */	208	/* Start turning the servo */
209	turnDirection = rxMsg.Data.bytes[1];	209	turnDirection = rxMsg.Data.bytes[1];
210	servoToTurn = rxMsg.Data.bytes[2];	210	servoToTurn = rxMsg.Data.bytes[2];
211	servoFeed_timeStamp = Timebase_CurrentTime();	211	servoFeed_timeStamp = Timebase_CurrentTime();
212		212
213	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){	213	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){
214	/* Stop turning */	214	/* Stop turning */
215	turnDirection = BLINDS_TURN_STOP;	215	turnDirection = BLINDS_TURN_STOP;
216		216
217	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){	217	}else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
218	/* Send blinds status to CAN */	218	/* Send blinds status to CAN */
219	boardTemperature = getTC1047temperature();	219	boardTemperature = getTC1047temperature();
220		220
221	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){	221	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
222	blindsFailsafe();	222	blindsFailsafe();
223	}else{	223	}else{
224	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;	224	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
225	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;	225	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
226	txMsg.Data.bytes[2] = 0x00;	226	txMsg.Data.bytes[2] = 0x00;
227	#if NUMBER_OF_RCS >0	227	#if NUMBER_OF_RCS >0
228	txMsg.Data.bytes[3] = getPosition(1);	228	txMsg.Data.bytes[3] = getPosition(1);
229	#endif	229	#endif
230	#if NUMBER_OF_RCS >1	230	#if NUMBER_OF_RCS >1
231	txMsg.Data.bytes[4] = getPosition(2);	231	txMsg.Data.bytes[4] = getPosition(2);
232	#endif	232	#endif
233	#if NUMBER_OF_RCS >2	233	#if NUMBER_OF_RCS >2
234	txMsg.Data.bytes[5] = getPosition(3);	234	txMsg.Data.bytes[5] = getPosition(3);
235	#endif	235	#endif
236	txMsg.DataLength = NUMBER_OF_RCS+3;	236	txMsg.DataLength = NUMBER_OF_RCS+3;
237	BIOS_CanSend(&txMsg);	237	BIOS_CanSend(&txMsg);
238	}	238	}
239	}	239	}
240	msg_received = 0;	240	msg_received = 0;
241	}	241	}
242		242
243	if( (Timebase_CurrentTime() - timeStamp) >= STATUS_SEND_PERIOD ){	243	if( (Timebase_CurrentTime() - timeStamp) >= STATUS_SEND_PERIOD ){
244	timeStamp = Timebase_CurrentTime();	244	timeStamp = Timebase_CurrentTime();
245	/* Send blinds status to CAN */	245	/* Send blinds status to CAN */
246		246
247	boardTemperature = getTC1047temperature();	247	boardTemperature = getTC1047temperature();
248		248
249	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){	249	if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
250	blindsFailsafe();	250	blindsFailsafe();
251	}else{	251	}else{
252	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;	252	txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
253	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;	253	txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
254	txMsg.Data.bytes[2] = 0x00;	254	txMsg.Data.bytes[2] = 0x00;
255	#if NUMBER_OF_RCS >0	255	#if NUMBER_OF_RCS >0
256	txMsg.Data.bytes[3] = getPosition(1);	256	txMsg.Data.bytes[3] = getPosition(1);
257	#endif	257	#endif
258	#if NUMBER_OF_RCS >1	258	#if NUMBER_OF_RCS >1
259	txMsg.Data.bytes[4] = getPosition(2);	259	txMsg.Data.bytes[4] = getPosition(2);
260	#endif	260	#endif
261	#if NUMBER_OF_RCS >2	261	#if NUMBER_OF_RCS >2
262	txMsg.Data.bytes[5] = getPosition(3);	262	txMsg.Data.bytes[5] = getPosition(3);
263	#endif	263	#endif
264	txMsg.DataLength = NUMBER_OF_RCS+3;	264	txMsg.DataLength = NUMBER_OF_RCS+3;
265		265
266	BIOS_CanSend(&txMsg);	266	BIOS_CanSend(&txMsg);
267	}	267	}
268	}	268	}
269	/* If start turning servo */	269	/* If start turning servo */
270	if( turnDirection ){	270	if( turnDirection ){
271	if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){	271	if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
272	timeStampTurn = Timebase_CurrentTime();	272	timeStampTurn = Timebase_CurrentTime();
273	/* Clockwise or counterclockwise? */	273	/* Clockwise or counterclockwise? */
274	if( turnDirection == BLINDS_TURN_LEFT ){	274	if( turnDirection == BLINDS_TURN_LEFT ){
275	alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);	275	alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);
276	}else if( turnDirection == BLINDS_TURN_RIGHT ){	276	}else if( turnDirection == BLINDS_TURN_RIGHT ){
277	alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);	277	alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);
278	}	278	}
279	}	279	}
280	}	280	}
281	/* Turn off the servo current feed */	281	/* Turn off the servo current feed */
282	if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){	282	if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
283	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);	283	SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
284	}else{	284	}else{
285	SERVO_FEED_PORT \|= (1<<SERVO_FEED_PIN);	285	SERVO_FEED_PORT \|= (1<<SERVO_FEED_PIN);
286	}	286	}
287	}	287	}
288	return 0;	288	return 0;
289	}	289	}
290		290
291	void blindsFailsafe()	291	void blindsFailsafe()
292	{	292	{
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
298		298
299	while(1){	299	while(1){
300	/* check if any messages have been received */	300	/* check if any messages have been received */
301	while(msg_received) {	301	while(msg_received) {
302	/* This relay is adressed*/	302	/* This relay is adressed*/
303	if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){	303	if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){
304	/* Return from failsafe mode */	304	/* Return from failsafe mode */
305	return;	305	return;
306	}	306	}
307	}	307	}
308		308
309	if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){	309	if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
310	timeStamp = Timebase_CurrentTime();	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;
316	txMsg.Data.bytes[2] = FAILSAFE_MODE;	316	txMsg.Data.bytes[2] = FAILSAFE_MODE;
317	#if NUMBER_OF_RCS >0	317	#if NUMBER_OF_RCS >0
318	txMsg.Data.bytes[3] = getPosition(1);	318	txMsg.Data.bytes[3] = getPosition(1);
319	#endif	319	#endif
320	#if NUMBER_OF_RCS >1	320	#if NUMBER_OF_RCS >1
321	txMsg.Data.bytes[4] = getPosition(2);	321	txMsg.Data.bytes[4] = getPosition(2);
322	#endif	322	#endif
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_CanSend(&txMsg);	328	BIOS_CanSend(&txMsg);
329	}	329	}
330	}	330	}
331	}	331	}
332		332
333		333

Subversion Repositories HomeAutomation

(root)/trunk/EmbeddedSoftware/AVR/application/CanServo/main.c @ 403 – Rev 388 → 392