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

 /**
- * CanServo.
+ * CAN Servo.
- * 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 <inttypes.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>
+//#include <funcdefs.h>
 /* defines */
+#define SERVOID_1 0xabc
+#define SERVOID_2 0xbcd
+#define SERVOID_3 0xcde
-#define GROUP_ID    0x01L // FIXME
+//#define GROUP_ID  0x01L // FIXME
 #define FAILSAFE_MODE       0x0F
-#define FAILSAFE_TRESHOLD   0x60  /* Degrees celcius when nodeBlinds goes into failsafe mode */
+#define FAILSAFE_TRESHOLD   50  // Degrees celcius when nodeBlinds goes into failsafe mode
-#define BLINDS_CMD_ABS      0x01 /* Absolute position */
+#define BLINDS_CMD_ABS      0x01 // Absolute position
-#define BLINDS_CMD_REL      0x02 /* Relative position */
+#define BLINDS_CMD_REL      0x02 // Relative position
-#define BLINDS_CMD_START    0x03 /* Start turning RC-servo */
+#define BLINDS_CMD_START    0x03 // Start turning RC-servo
-#define BLINDS_CMD_STOP     0x04 /* Stop 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 STEPS_PER_TURN_PERIOD   10 // number of steps for each adjustment
-#define TURN_PERIOD             100 /* ms */
+#define TURN_PERIOD             100 // ms
-#define STATUS_SEND_PERIOD      5000 /* ms */
+#define STATUS_SEND_PERIOD      5000 // ms
-#define FAILSAFE_SEND_PERIOD    10000 /* ms */
+#define FAILSAFE_SEND_PERIOD    10000 // ms
-#define BOUNCE_TIME             10 /* ms */
+#define BOUNCE_TIME             10 // ms
-#define SERVO_FEED_TIME         2000 /* ms */
+#define SERVO_FEED_TIME         5000 // ms
 #define BUTTON1                 1
 #define BUTTON2                 2
 #define SERVO_FEED_PORT         PORTD
 #define SERVO_FEED_DDR          DDRD
 #define SERVO_FEED_PIN          PD2
+#define APP_TYPE    CAN_APPTYPES_SERVO
+#define APP_VERSION 0x0001
 /*-------------------------------------------------------------------------
  * Global variables
  * -----------------------------------------------------------------------*/
 /*#if defined(TWO_BUTTON_MODE)
 Can_Message_t txMsg_Button;
 #endif*/
-uint8_t msg_received = 0;
+volatile uint8_t rxMsgFull;
-Can_Message_t txMsg;
-Can_Message_t rxMsg;
+volatile 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;
+// CAN callback function
-    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++){
+    if(!rxMsgFull){
-                    rxMsg.Data.bytes[m] = msg->Data.bytes[m];
+        memcpy((void*)&rxMsg, msg, sizeof(rxMsg));
-                }
-            msg_received = 1;
+        rxMsgFull = 1;
-            }
-        }
+    }
+}
 /*----------------------------------------------------------------------------
  * Interrupt routines
  * -------------------------------------------------------------------------*/
 /*-----------------------------------------------------------------------------
  * Main Program
  *---------------------------------------------------------------------------*/
 int main(void) {
+    uint32_t act;
+    int8_t m;
+    uint8_t acttype, temp_id;
+    uint16_t servoid;
     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;
+    Can_Message_t txMsg;
+    // Set up callback for CAN messages
+    BIOS_CanCallback = &can_receive;
     sei();
     Timebase_Init();
     adcTemperatureInit();
     rcServoInit();
 #if defined(TWO_BUTTON_MODE)
     /*
      * Initialize buttons
      * It is possible to use ie. sensors instead
      * but then change this
 #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.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
     txMsg.RemoteFlag = 0;
     txMsg.ExtendedFlag = 1;
+    txMsg.DataLength = 4;
+    txMsg.Data.words[0] = APP_TYPE;
+    txMsg.Data.words[1] = APP_VERSION;
+    BIOS_CanSend(&txMsg); // Send startup message
-    txMsg.Id = (CLASS_SNS << CLASS_MASK_BITS)|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)|(NODE_ID << SID_MASK_BITS);
+    txMsg.Id = 0x08000c0b; //(CLASS_SNS << CLASS_MASK_BITS)|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)|(NODE_ID << SID_MASK_BITS);
+    // Main loop
+    while (1) {
+        // check if any messages have been received
+        if(rxMsgFull){
+            if( ((rxMsg.Id & CAN_MASK_CLASS)>>CAN_SHIFT_CLASS) == CAN_ACT ){
+                // Actuator package
+                acttype = (uint8_t)((rxMsg.Id & CAN_MASK_ACT_FUNCC) >> CAN_SHIFT_ACT_FUNCC);
+                servoid = (uint16_t)((rxMsg.Id & CAN_MASK_ACT_NID) >> CAN_SHIFT_ACT_NID);
+        // Check if it is command to control this servo
+                if(acttype == ACT_FUNCC_SERVO && (servoid == SERVOID_1 || servoid == SERVOID_2 || servoid == SERVOID_3)){
+                    // This node that control a servo is adressed
+                    if(servoid == SERVOID_1){
+                        temp_id = 1; // First servo
+                    }else if(servoid == SERVOID_2){
+                        temp_id = 2; // and so on
+                    }else if(servoid == SERVOID_3){
+                        temp_id = 3;
+                    }
+                    if(rxMsg.DataLength == 1 || (rxMsg.DataLength == 2 && rxMsg.Data.bytes[1] == BLINDS_CMD_ABS)){
-    /* Set up callback for CAN messages */
+                        // Set absolute position to servo
+                        setPosition( rxMsg.Data.bytes[0], temp_id );
-    BIOS_CanCallback = &can_receive;
+                        servoFeed_timeStamp = Timebase_CurrentTime();
+                    }else if(rxMsg.DataLength == 2){
+                        if(rxMsg.Data.bytes[1] == BLINDS_CMD_REL){
+                            // Set relative position to servo
+                            alterPosition( rxMsg.Data.bytes[0], temp_id );
+                            servoFeed_timeStamp = Timebase_CurrentTime();
-    /* 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 ){
+                        }else if(rxMsg.Data.bytes[1] == BLINDS_CMD_START ){
-                /* Set absolute position to servo */
+                            // Start turning the servo
-                setPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
-                servoFeed_timeStamp = Timebase_CurrentTime();
-            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_REL){
+                            turnDirection = rxMsg.Data.bytes[0]; // FIXME riktning beroende på positivt eller negativt tal?
-                /* Set relative position to servo */
+                            servoToTurn = temp_id;
-                alterPosition( rxMsg.Data.bytes[1], rxMsg.Data.bytes[2] );
-                servoFeed_timeStamp = Timebase_CurrentTime();
+                            servoFeed_timeStamp = Timebase_CurrentTime();
-            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_START ){
+                        }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STOP ){
-                /* Start turning the servo */
+                            // Stop turning
-                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;
+                            turnDirection = BLINDS_TURN_STOP;
-            }else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
+                        }/*else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
-                /* Send blinds status to CAN */
+                            // Send blinds status to CAN
-                boardTemperature = getTC1047temperature();
+                            boardTemperature = getTC1047temperature();
-                if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
+                            if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
-                    blindsFailsafe();
+                                blindsFailsafe();
-                }else{
+                            }else{
-                    txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
+                            txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
-                    txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
+                            txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
-                    txMsg.Data.bytes[2] = 0x00;
+                            txMsg.Data.bytes[2] = 0x00;
-#if NUMBER_OF_RCS >0
-                    txMsg.Data.bytes[3] = getPosition(1);
+                            txMsg.Data.bytes[3] = getPosition(1);
-#endif
-#if NUMBER_OF_RCS >1
-                    txMsg.Data.bytes[4] = getPosition(2);
+                            txMsg.Data.bytes[4] = getPosition(2);
-#endif
-#if NUMBER_OF_RCS >2
-                    txMsg.Data.bytes[5] = getPosition(3);
+                            txMsg.Data.bytes[5] = getPosition(3);
-#endif
-                    txMsg.DataLength = NUMBER_OF_RCS+3;
+                            txMsg.DataLength = 6;
-                    BIOS_CanSend(&txMsg);
+                            BIOS_CanSend(&txMsg);
+                        }*/
+                    }
+                }
+            }
-                msg_received = 0;
+            rxMsgFull = 0;
+        }
         if( (Timebase_CurrentTime() - timeStamp)  >= STATUS_SEND_PERIOD ){
             timeStamp = Timebase_CurrentTime();
-            /* Send blinds status to CAN */
+            // Send blinds status to CAN periodically
             boardTemperature = getTC1047temperature();
             if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
                 blindsFailsafe();
-            }else{
+                }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;
+                txMsg.DataLength = 6;
                 BIOS_CanSend(&txMsg);
+            }
         }
-        /* If start turning servo */
+        // If start turning servo
-        if( turnDirection ){
+/*      if( turnDirection ){
             if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
                 timeStampTurn = Timebase_CurrentTime();
-                /* Clockwise or counterclockwise? */
+                // 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 */
+        // Turn off the servo current feed
         if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
             SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
+            servoDisable();
         }else{
             SERVO_FEED_PORT |= (1<<SERVO_FEED_PIN);
+            servoEnable();
-        }
+        }*/
+    }
     return 0;
 }
 void blindsFailsafe()
+{
-    uint32_t boardTemperature = 0, timeStamp = 0;
+/*  uint32_t boardTemperature = 0, timeStamp = 0;
-    /* Turn of servo current feed */
+    // Turn of servo current feed
     SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
     while(1){
-        /* check if any messages have been received */
+        // check if any messages have been received
-        while(msg_received) {
+        while(rxMsgFull) {
-            /* This relay is adressed*/
+            // This relay is adressed
             if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){
-                /* Return from failsafe mode */
+                // Return from failsafe mode
                 return;
+            }
+        }
         if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
             timeStamp = Timebase_CurrentTime();
-            /* Send relay status to CAN */
+            // 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;
 #endif
             txMsg.DataLength = NUMBER_OF_RCS+3;
             BIOS_CanSend(&txMsg);
+        }
-    }
+    }*/
+}

Subversion Repositories HomeAutomation

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