Subversion Repositories HomeAutomation

Rev

Rev 551 | 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 <stdio.h>
  21.  
  22. /* lib files */
  23. #include <bios.h>
  24. #include <config.h>
  25. //#include <settings.h> //Migrated to config.inc
  26.  
  27. #include <drivers/timer/timebase.h>
  28. #include <drivers/sensor/tc1047/tc1047.h>
  29. #include <drivers/actuator/rc-servo/rc_servo.h>
  30.  
  31. /*-----------------------------------------------------------------------------
  32.  * Defines
  33.  *---------------------------------------------------------------------------*/
  34. #define FAILSAFE_MODE       0x0F
  35. #define BLINDS_CMD_ABS      0x01 // Absolute position
  36. #define BLINDS_CMD_REL      0x02 // Relative position
  37. #define BLINDS_CMD_START    0x03 // Start turning RC-servo
  38. #define BLINDS_CMD_STOP     0x04 // Stop turning RC-servo
  39. #define BLINDS_CMD_RESET    0x05
  40. #define BLINDS_CMD_STATUS   0x06
  41.  
  42. #define BLINDS_TURN_STOP    0x00
  43. #define BLINDS_TURN_CCW     0x01
  44. #define BLINDS_TURN_CW      0x02
  45.  
  46. #define STATUS_SEND_PERIOD      5000 // ms
  47. #define FAILSAFE_SEND_PERIOD    10000 // ms
  48. #define BOUNCE_TIME             10 // ms
  49. #define BUTTON1                 1
  50. #define BUTTON2                 2
  51.  
  52. #define SERVO_FEED_PORT         PORTC
  53. #define SERVO_FEED_DDR          DDRC
  54. #define SERVO_FEED_PIN          PC0
  55.  
  56. #define APP_TYPE    CAN_APPTYPES_SERVO
  57. #define APP_VERSION 0x0001
  58.  
  59.  
  60. /*-------------------------------------------------------------------------
  61.  * Global variables
  62.  * -----------------------------------------------------------------------*/
  63. /*#if defined(TWO_BUTTON_MODE)
  64. Can_Message_t txMsg_Button;
  65. #endif*/
  66.  
  67. volatile uint8_t rxMsgFull;
  68.  
  69. volatile Can_Message_t rxMsg;
  70.  
  71. /*----------------------------------------------------------------------------
  72.  * Functions
  73.  * -------------------------------------------------------------------------*/
  74. void blindsFailsafe(void);
  75.  
  76. void can_receive(Can_Message_t *msg){
  77. // CAN callback function
  78.     if(!rxMsgFull){
  79.         memcpy((void*)&rxMsg, msg, sizeof(rxMsg));
  80.         rxMsgFull = 1;
  81.     }
  82. }
  83. /*----------------------------------------------------------------------------
  84.  * Interrupt routines
  85.  * -------------------------------------------------------------------------*/
  86. #if defined(TWO_BUTTON_MODE)
  87. #error two button mode is still untested
  88. ISR( PCINT2_vect )
  89. {
  90.     /*
  91.      * Auxiliary button pressed and released
  92.      * Check time between press and release to eliminate bounces
  93.      */
  94.     static uint32_t buttonTime[2];
  95.  
  96.     txMsg_Button.RemoteFlag = 0;
  97.     txMsg_Button.ExtendedFlag = 1;
  98.  
  99.     if( (PIND & (1<<PD1)) == 0){
  100.         /* Button pressed */
  101.         buttonTime[0] = Timebase_CurrentTime();
  102.     }else if( (PIND & (1<<PD1)) == 1){
  103.         if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
  104.             /* No bounce, send message */
  105.             txMsg_Button.Id = BUTTON_ID;
  106.             txMsg_Button.DataLength = 1;
  107.             txMsg_Button.Data.bytes[0] = BUTTON1;
  108.             return;
  109.         }else{
  110.             /* Just bounces, ignore it */
  111.             return;
  112.         }
  113.     }
  114.     if( (PIND & (1<<PD2)) == 0){
  115.         /* Button pressed */
  116.         buttonTime[1] = Timebase_CurrentTime();
  117.     }else if( (PIND & (1<<PD2)) == 1){
  118.         if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){
  119.             /* No bounce, send message */
  120.             txMsg_Button.Id = BUTTON_ID;
  121.             txMsg_Button.DataLength = 1;
  122.             txMsg_Button.Data.bytes[0] = BUTTON2;
  123.             return;
  124.         }else{
  125.             /* Just bounces, ignore it */
  126.             return;
  127.         }
  128.     }
  129. }
  130. #endif
  131.  
  132. /*-----------------------------------------------------------------------------
  133.  * Main Program
  134.  *---------------------------------------------------------------------------*/
  135. int main(void) {
  136.     uint8_t temp_id, servoid;
  137.     uint16_t acttype;
  138.  
  139.     uint8_t //blindsStatus, /* Position (-128 to 127) */
  140.             servoToTurn,
  141.             turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
  142.     uint32_t boardTemperature = 0;
  143.     uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
  144.  
  145.     Can_Message_t txMsg;
  146.  
  147.     // Set up callback for CAN messages
  148.     BIOS_CanCallback = &can_receive;
  149.  
  150.     sei();
  151.  
  152.     Timebase_Init();
  153.     adcTemperatureInit();
  154.     rcServoInit();
  155.  
  156. #if defined(TWO_BUTTON_MODE)
  157.     /*
  158.      * Initialize buttons
  159.      * It is possible to use ie. sensors instead
  160.      * but then change this
  161.      */
  162.     /* Set as input and enable pull-up */
  163.     DDRD &= ~( (1<<DDD1)|(1<<DDD2) );
  164.     PORTD |= (1<<PD1)|(1<<PD2);
  165.  
  166.     /* Enable IO-pin interrupt */
  167.     PCICR |= (1<<PCIE2);
  168.     /* Unmask PD1 and SERVO_FEED_PIN */
  169.     PCMSK2 |= (1<<PCINT16) | (1<<PCINT17);
  170. #endif
  171.     /* Set output and turn off the servo current feed */
  172.     //SERVO_FEED_DDR |= (1<<SERVO_FEED_PIN);
  173.     //SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  174.  
  175.     txMsg.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
  176.     txMsg.RemoteFlag = 0;
  177.     txMsg.ExtendedFlag = 1;
  178.     txMsg.DataLength = 4;
  179.     txMsg.Data.words[0] = APP_TYPE;
  180.     txMsg.Data.words[1] = APP_VERSION;
  181.  
  182.     BIOS_CanSend(&txMsg); // Send startup message
  183.    
  184.     //set up status message
  185.     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));
  186.  
  187.     // Main loop
  188.     while (1) {
  189.         // check if any messages have been received
  190.         if(rxMsgFull){
  191.             if( ((rxMsg.Id & CAN_MASK_CLASS)>>CAN_SHIFT_CLASS) == CAN_ACT ){
  192.                 // Actuator package
  193.                 acttype = (uint16_t)((rxMsg.Id & CAN_MASK_ACT_TYPE) >> CAN_SHIFT_ACT_TYPE);
  194.                 servoid = (uint8_t)((rxMsg.Id & CAN_MASK_ACT_ID) >> CAN_SHIFT_ACT_ID);
  195.  
  196.         // Check if it is command to control this servo
  197.                 if(acttype == ACT_TYPE_SERVO && (servoid == SERVOID_1 || servoid == SERVOID_2 || servoid == SERVOID_3)){
  198.                     // This node that control a servo is adressed
  199.                     if(servoid == SERVOID_1){
  200.                         temp_id = 1; // First servo
  201.                     }else if(servoid == SERVOID_2){
  202.                         temp_id = 2; // and so on
  203.                     }else if(servoid == SERVOID_3){
  204.                         temp_id = 3;
  205.                     }else{
  206.                         temp_id = 0;
  207.                     }
  208.  
  209.                     if(rxMsg.DataLength == 1 || (rxMsg.DataLength == 2 && rxMsg.Data.bytes[1] == BLINDS_CMD_ABS)){
  210.                         // Set absolute position to servo
  211.  
  212.                         setPosition( rxMsg.Data.bytes[0], temp_id );
  213.                         servoFeed_timeStamp = Timebase_CurrentTime();
  214.  
  215.                     }else if(rxMsg.DataLength == 2){
  216.                         if(rxMsg.Data.bytes[1] == BLINDS_CMD_REL){
  217.                             // Set relative position to servo
  218.  
  219.                             alterPosition( rxMsg.Data.bytes[0], temp_id );
  220.                             servoFeed_timeStamp = Timebase_CurrentTime();
  221.  
  222.                         }else if(rxMsg.Data.bytes[1] == BLINDS_CMD_START ){
  223.                             // Start turning the servo
  224.  
  225.                             turnDirection = (rxMsg.Data.bytes[0]&0x80)?BLINDS_TURN_CW:BLINDS_TURN_CCW; // FIXME riktning beroende på positivt eller negativt tal?
  226.                             servoToTurn = temp_id;
  227.                             servoFeed_timeStamp = Timebase_CurrentTime();
  228.  
  229.                         }else if(rxMsg.Data.bytes[1] == BLINDS_CMD_STOP ){
  230.                             // Stop turning
  231.  
  232.                             turnDirection = BLINDS_TURN_STOP;
  233.  
  234.                         }/*else if(rxMsg.Data.bytes[0] == BLINDS_CMD_STATUS){
  235.                             // Send blinds status to CAN
  236.                             boardTemperature = getTC1047temperature();
  237.  
  238.                             if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
  239.                                 blindsFailsafe();
  240.                             }else{
  241.                             txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  242.                             txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  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();
  262.  
  263.             if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
  264.                 blindsFailsafe();
  265.                 }else{
  266.                 txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  267.                 txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  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. /*  uint32_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();
  325.  
  326.             txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  327.             txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  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.