Subversion Repositories HomeAutomation

Rev

Rev 797 | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1. /**
  2.  * CAN Servo.
  3.  *
  4.  * Input: 0 to 255
  5.  * Position msg: <byte0 BLINDS_CMD> <byte1 position> <byte2 servo number>
  6.  * Turning msg: <byte0 START/STOP> <byte 1 direction> <byte2 servo number>
  7.  *
  8.  * @date    2007-01-01
  9.  * @author  Erik Larsson
  10.  *
  11.  */
  12.  
  13. /*-----------------------------------------------------------------------------
  14.  * Includes
  15.  *---------------------------------------------------------------------------*/
  16. /* system files */
  17. #include <avr/io.h>
  18. #include <avr/interrupt.h>
  19. #include <inttypes.h>
  20. #include <string.h>
  21. #include <stdio.h>
  22.  
  23. /* lib files */
  24. #include <bios.h>
  25. #include <config.h>
  26. //#include <settings.h> //Migrated to config.inc
  27.  
  28. #include <drivers/timer/timebase.h>
  29. #include <drivers/sensor/tc1047/tc1047.h>
  30. #include <drivers/actuator/rc-servo/rc_servo.h>
  31.  
  32. /*-----------------------------------------------------------------------------
  33.  * Defines
  34.  *---------------------------------------------------------------------------*/
  35. #define FAILSAFE_MODE       0x0F
  36. #define BLINDS_CMD_ABS      0x01 // Absolute position
  37. #define BLINDS_CMD_REL      0x02 // Relative position
  38. #define BLINDS_CMD_START    0x03 // Start turning RC-servo
  39. #define BLINDS_CMD_STOP     0x04 // Stop turning RC-servo
  40. #define BLINDS_CMD_RESET    0x05
  41. #define BLINDS_CMD_STATUS   0x06
  42.  
  43. #define BLINDS_TURN_STOP    0x00
  44. #define BLINDS_TURN_CCW     0x01
  45. #define BLINDS_TURN_CW      0x02
  46.  
  47. #define STATUS_SEND_PERIOD      5000 // ms
  48. #define FAILSAFE_SEND_PERIOD    10000 // ms
  49. #define BOUNCE_TIME             10 // ms
  50. #define BUTTON1                 1
  51. #define BUTTON2                 2
  52.  
  53. #define SERVO_FEED_PORT         PORTC
  54. #define SERVO_FEED_DDR          DDRC
  55. #define SERVO_FEED_PIN          PC0
  56.  
  57. #define APP_TYPE    CAN_APPTYPES_SERVO
  58. #define APP_VERSION 0x0001
  59.  
  60.  
  61. /*-------------------------------------------------------------------------
  62.  * Global variables
  63.  * -----------------------------------------------------------------------*/
  64. /*#if defined(TWO_BUTTON_MODE)
  65. Can_Message_t txMsg_Button;
  66. #endif*/
  67.  
  68. volatile uint8_t rxMsgFull;
  69.  
  70. volatile Can_Message_t rxMsg;
  71.  
  72. /*----------------------------------------------------------------------------
  73.  * Functions
  74.  * -------------------------------------------------------------------------*/
  75. void blindsFailsafe(void);
  76.  
  77. void can_receive(Can_Message_t *msg){
  78. // CAN callback function
  79.     if(!rxMsgFull){
  80.         memcpy((void*)&rxMsg, msg, sizeof(rxMsg));
  81.         rxMsgFull = 1;
  82.     }
  83. }
  84. /*----------------------------------------------------------------------------
  85.  * Interrupt routines
  86.  * -------------------------------------------------------------------------*/
  87. #if defined(TWO_BUTTON_MODE)
  88. #error two button mode is still untested
  89. ISR( PCINT2_vect )
  90. {
  91.     /*
  92.      * Auxiliary button pressed and released
  93.      * Check time between press and release to eliminate bounces
  94.      */
  95.     static uint32_t buttonTime[2];
  96.  
  97.     txMsg_Button.RemoteFlag = 0;
  98.     txMsg_Button.ExtendedFlag = 1;
  99.  
  100.     if( (PIND & (1<<PD1)) == 0){
  101.         /* Button pressed */
  102.         buttonTime[0] = Timebase_CurrentTime();
  103.     }else if( (PIND & (1<<PD1)) == 1){
  104.         if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
  105.             /* No bounce, send message */
  106.             txMsg_Button.Id = BUTTON_ID;
  107.             txMsg_Button.DataLength = 1;
  108.             txMsg_Button.Data.bytes[0] = BUTTON1;
  109.             return;
  110.         }else{
  111.             /* Just bounces, ignore it */
  112.             return;
  113.         }
  114.     }
  115.     if( (PIND & (1<<PD2)) == 0){
  116.         /* Button pressed */
  117.         buttonTime[1] = Timebase_CurrentTime();
  118.     }else if( (PIND & (1<<PD2)) == 1){
  119.         if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){
  120.             /* No bounce, send message */
  121.             txMsg_Button.Id = BUTTON_ID;
  122.             txMsg_Button.DataLength = 1;
  123.             txMsg_Button.Data.bytes[0] = BUTTON2;
  124.             return;
  125.         }else{
  126.             /* Just bounces, ignore it */
  127.             return;
  128.         }
  129.     }
  130. }
  131. #endif
  132.  
  133. /*-----------------------------------------------------------------------------
  134.  * Main Program
  135.  *---------------------------------------------------------------------------*/
  136. int main(void) {
  137.     uint8_t temp_id, servoid;
  138.     uint16_t acttype;
  139.  
  140.     uint8_t //blindsStatus, /* Position (-128 to 127) */
  141.             servoToTurn,
  142.             turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
  143.     uint16_t boardTemperature = 0;
  144.     uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
  145.  
  146.     Can_Message_t txMsg;
  147.  
  148.     // Set up callback for CAN messages
  149.     BIOS_CanCallback = &can_receive;
  150.  
  151.     sei();
  152.  
  153.     Timebase_Init();
  154.     adcTemperatureInit();
  155.     rcServoInit();
  156.  
  157. #if defined(TWO_BUTTON_MODE)
  158.     /*
  159.      * Initialize buttons
  160.      * It is possible to use ie. sensors instead
  161.      * but then change this
  162.      */
  163.     /* Set as input and enable pull-up */
  164.     DDRD &= ~( (1<<DDD1)|(1<<DDD2) );
  165.     PORTD |= (1<<PD1)|(1<<PD2);
  166.  
  167.     /* Enable IO-pin interrupt */
  168.     PCICR |= (1<<PCIE2);
  169.     /* Unmask PD1 and SERVO_FEED_PIN */
  170.     PCMSK2 |= (1<<PCINT16) | (1<<PCINT17);
  171. #endif
  172.     /* Set output and turn off the servo current feed */
  173.     //SERVO_FEED_DDR |= (1<<SERVO_FEED_PIN);
  174.     //SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  175.  
  176.     txMsg.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
  177.     txMsg.RemoteFlag = 0;
  178.     txMsg.ExtendedFlag = 1;
  179.     txMsg.DataLength = 4;
  180.     txMsg.Data.words[0] = APP_TYPE;
  181.     txMsg.Data.words[1] = APP_VERSION;
  182.  
  183.     BIOS_CanSend(&txMsg); // Send startup message
  184.    
  185.     //set up status message
  186.     txMsg.Id = ((CAN_SNS << CAN_SHIFT_CLASS) | (SNS_TYPE_STATUS << CAN_SHIFT_SNS_TYPE) | (SNS_ID_SERVO_STATUS << CAN_SHIFT_SNS_ID) | (NODE_ID << CAN_SHIFT_SNS_SID));
  187.  
  188.     // Main loop
  189.     while (1) {
  190.         // check if any messages have been received
  191.         if(rxMsgFull){
  192.             if( ((rxMsg.Id & CAN_MASK_CLASS)>>CAN_SHIFT_CLASS) == CAN_ACT ){
  193.                 // Actuator package
  194.                 acttype = (uint16_t)((rxMsg.Id & CAN_MASK_ACT_TYPE) >> CAN_SHIFT_ACT_TYPE);
  195.                 servoid = (uint8_t)((rxMsg.Id & CAN_MASK_ACT_ID) >> CAN_SHIFT_ACT_ID);
  196.  
  197.         // Check if it is command to control this servo
  198.                 if(acttype == ACT_TYPE_SERVO && (servoid == SERVOID_1 || servoid == SERVOID_2 || servoid == SERVOID_3)){
  199.                     // This node that control a servo is adressed
  200.                     if(servoid == SERVOID_1){
  201.                         temp_id = 1; // First servo
  202.                     }else if(servoid == SERVOID_2){
  203.                         temp_id = 2; // and so on
  204.                     }else if(servoid == SERVOID_3){
  205.                         temp_id = 3;
  206.                     }else{
  207.                         temp_id = 0;
  208.                     }
  209.  
  210.                     if(rxMsg.DataLength == 1 || (rxMsg.DataLength == 2 && rxMsg.Data.bytes[1] == BLINDS_CMD_ABS)){
  211.                         // Set absolute position to servo
  212.  
  213.                         setPosition( rxMsg.Data.bytes[0], temp_id );
  214.                         servoFeed_timeStamp = Timebase_CurrentTime();
  215.  
  216.                     }else if(rxMsg.DataLength == 2){
  217.                         if(rxMsg.Data.bytes[1] == BLINDS_CMD_REL){
  218.                             // Set relative position to servo
  219.  
  220.                             alterPosition( rxMsg.Data.bytes[0], temp_id );
  221.                             servoFeed_timeStamp = Timebase_CurrentTime();
  222.  
  223.                         }else if(rxMsg.Data.bytes[1] == BLINDS_CMD_START ){
  224.                             // Start turning the servo
  225.  
  226.                             turnDirection = (rxMsg.Data.bytes[0]&0x80)?BLINDS_TURN_CW:BLINDS_TURN_CCW; // FIXME riktning beroende på positivt eller negativt tal?
  227.                             servoToTurn = temp_id;
  228.                             servoFeed_timeStamp = Timebase_CurrentTime();
  229.  
  230.                         }else if(rxMsg.Data.bytes[1] == BLINDS_CMD_STOP ){
  231.                             // Stop turning
  232.  
  233.                             turnDirection = BLINDS_TURN_STOP;
  234.  
  235.                         }/*else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
  236.                             // Send blinds status to CAN
  237.                             boardTemperature = getTC1047temperature(TC1047_AD);
  238.                             if (boardTemperature >= FAILSAFE_TRESHOLD ){
  239.                                 blindsFailsafe();
  240.                             } else {
  241.                             txMsg.Data.bytes[0] = boardTemperature&0&ff;
  242.                             txMsg.Data.bytes[1] = ((boardTemperature>>8)&0xff);
  243.                             txMsg.Data.bytes[2] = 0x00;
  244.                             txMsg.Data.bytes[3] = getPosition(1);
  245.                             txMsg.Data.bytes[4] = getPosition(2);
  246.                             txMsg.Data.bytes[5] = getPosition(3);
  247.                             txMsg.DataLength = 6;
  248.                             BIOS_CanSend(&txMsg);
  249.                         }*/
  250.                     }
  251.                 }
  252.             }
  253.  
  254.             rxMsgFull = 0;
  255.         }
  256.  
  257.         if( (Timebase_CurrentTime() - timeStamp)  >= STATUS_SEND_PERIOD ){
  258.             timeStamp = Timebase_CurrentTime();
  259.             // Send blinds status to CAN periodically
  260.  
  261.             boardTemperature = getTC1047temperature(TC1047_AD);
  262.  
  263.             if ( boardTemperature >= FAILSAFE_TRESHOLD ){
  264.                 blindsFailsafe();
  265.                 }else{
  266.                 txMsg.Data.bytes[0] = boardTemperature&0xff;
  267.                 txMsg.Data.bytes[1] = ((boardTemperature>>8)&0xff);
  268.                 txMsg.Data.bytes[2] = 0x00;
  269.                 txMsg.Data.bytes[3] = getPosition(1);
  270.                 txMsg.Data.bytes[4] = getPosition(2);
  271.                 txMsg.Data.bytes[5] = getPosition(3);
  272.                 txMsg.DataLength = 6;
  273.  
  274.                 BIOS_CanSend(&txMsg);
  275.             }
  276.         }
  277.         // If start turning servo
  278.         if( turnDirection ){
  279.             if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
  280.                 timeStampTurn = Timebase_CurrentTime();
  281.                 // Clockwise or counterclockwise?
  282.                 if( turnDirection == BLINDS_TURN_CCW ){
  283.                     alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);
  284.                 }else if( turnDirection == BLINDS_TURN_CW ){
  285.                     alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);
  286.                 }
  287.                 servoFeed_timeStamp = Timebase_CurrentTime();
  288.             }
  289.         }
  290.  
  291.         // Turn off the servo current feed after timeout
  292.         if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
  293.             SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  294.             servoDisable();
  295.         }else{
  296.             SERVO_FEED_PORT |= (1<<SERVO_FEED_PIN);
  297.             servoEnable();
  298.         }
  299.     }
  300.     return 0;
  301. }
  302.  
  303. void blindsFailsafe()
  304. {
  305. /*  uint16_t boardTemperature = 0, timeStamp = 0;
  306.  
  307.     // Turn of servo current feed
  308.     SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  309.  
  310.  
  311.     while(1){
  312.         // check if any messages have been received
  313.         while(rxMsgFull) {
  314.             // This relay is adressed
  315.             if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){
  316.                 // Return from failsafe mode
  317.                 return;
  318.             }
  319.         }
  320.  
  321.         if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
  322.             timeStamp = Timebase_CurrentTime();
  323.             // Send relay status to CAN
  324.             boardTemperature = getTC1047temperature(TC1047_AD);
  325.  
  326.             txMsg.Data.bytes[0] = boardTemperature&0xff;
  327.             txMsg.Data.bytes[1] = ((boardTemperature>>8)&0xff);
  328.             txMsg.Data.bytes[2] = FAILSAFE_MODE;
  329. #if NUMBER_OF_RCS >0
  330.             txMsg.Data.bytes[3] = getPosition(1);
  331. #endif
  332. #if NUMBER_OF_RCS >1
  333.             txMsg.Data.bytes[4] = getPosition(2);
  334. #endif
  335. #if NUMBER_OF_RCS >2
  336.             txMsg.Data.bytes[5] = getPosition(3);
  337. #endif
  338.             txMsg.DataLength = NUMBER_OF_RCS+3;
  339.  
  340.             BIOS_CanSend(&txMsg);
  341.         }
  342.     }*/
  343. }
  344.  
  345.