Subversion Repositories HomeAutomation

Rev

Rev 496 | Rev 502 | Go to most recent revision | 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>
  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 STEPS_PER_TURN_PERIOD   10 // number of steps for each adjustment
  47. #define TURN_PERIOD             100 // ms
  48. #define STATUS_SEND_PERIOD      5000 // ms
  49. #define FAILSAFE_SEND_PERIOD    10000 // ms
  50. #define BOUNCE_TIME             10 // ms
  51. #define BUTTON1                 1
  52. #define BUTTON2                 2
  53.  
  54. #define SERVO_FEED_PORT         PORTD
  55. #define SERVO_FEED_DDR          DDRD
  56. #define SERVO_FEED_PIN          PD2
  57.  
  58. #define APP_TYPE    CAN_APPTYPES_SERVO
  59. #define APP_VERSION 0x0001
  60.  
  61.  
  62. /*-------------------------------------------------------------------------
  63.  * Global variables
  64.  * -----------------------------------------------------------------------*/
  65. /*#if defined(TWO_BUTTON_MODE)
  66. Can_Message_t txMsg_Button;
  67. #endif*/
  68.  
  69. volatile uint8_t rxMsgFull;
  70.  
  71. volatile Can_Message_t rxMsg;
  72.  
  73. /*----------------------------------------------------------------------------
  74.  * Functions
  75.  * -------------------------------------------------------------------------*/
  76. void blindsFailsafe(void);
  77.  
  78. void can_receive(Can_Message_t *msg){
  79. // CAN callback function
  80.     if(!rxMsgFull){
  81.         memcpy((void*)&rxMsg, msg, sizeof(rxMsg));
  82.         rxMsgFull = 1;
  83.     }
  84. }
  85. /*----------------------------------------------------------------------------
  86.  * Interrupt routines
  87.  * -------------------------------------------------------------------------*/
  88. #if defined(TWO_BUTTON_MODE)
  89. #error two button mode is still untested
  90. ISR( PCINT2_vect )
  91. {
  92.     /*
  93.      * Auxiliary button pressed and released
  94.      * Check time between press and release to eliminate bounces
  95.      */
  96.     static uint32_t buttonTime[2];
  97.  
  98.     txMsg_Button.RemoteFlag = 0;
  99.     txMsg_Button.ExtendedFlag = 1;
  100.  
  101.     if( (PIND & (1<<PD1)) == 0){
  102.         /* Button pressed */
  103.         buttonTime[0] = Timebase_CurrentTime();
  104.     }else if( (PIND & (1<<PD1)) == 1){
  105.         if( Timebase_PassedTimeMillis( buttonTime[0] ) >= BOUNCE_TIME ){
  106.             /* No bounce, send message */
  107.             txMsg_Button.Id = BUTTON_ID;
  108.             txMsg_Button.DataLength = 1;
  109.             txMsg_Button.Data.bytes[0] = BUTTON1;
  110.             return;
  111.         }else{
  112.             /* Just bounces, ignore it */
  113.             return;
  114.         }
  115.     }
  116.     if( (PIND & (1<<PD2)) == 0){
  117.         /* Button pressed */
  118.         buttonTime[1] = Timebase_CurrentTime();
  119.     }else if( (PIND & (1<<PD2)) == 1){
  120.         if( Timebase_PassedTimeMillis( buttonTime[1] ) >= BOUNCE_TIME ){
  121.             /* No bounce, send message */
  122.             txMsg_Button.Id = BUTTON_ID;
  123.             txMsg_Button.DataLength = 1;
  124.             txMsg_Button.Data.bytes[0] = BUTTON2;
  125.             return;
  126.         }else{
  127.             /* Just bounces, ignore it */
  128.             return;
  129.         }
  130.     }
  131. }
  132. #endif
  133.  
  134. /*-----------------------------------------------------------------------------
  135.  * Main Program
  136.  *---------------------------------------------------------------------------*/
  137. int main(void) {
  138.     uint8_t temp_id;
  139.     uint16_t acttype, servoid;
  140.  
  141.     uint8_t //blindsStatus, /* Position (-128 to 127) */
  142.             servoToTurn,
  143.             turnDirection = BLINDS_TURN_STOP; /* Specifies direction to turn or stopped */
  144.     uint32_t boardTemperature = 0;
  145.     uint32_t timeStamp = 0, timeStampTurn = 0, servoFeed_timeStamp = 0;
  146.  
  147.     Can_Message_t txMsg;
  148.  
  149.     // Set up callback for CAN messages
  150.     BIOS_CanCallback = &can_receive;
  151.  
  152.     sei();
  153.  
  154.     Timebase_Init();
  155.     adcTemperatureInit();
  156.     rcServoInit();
  157.  
  158. #if defined(TWO_BUTTON_MODE)
  159.     /*
  160.      * Initialize buttons
  161.      * It is possible to use ie. sensors instead
  162.      * but then change this
  163.      */
  164.     /* Set as input and enable pull-up */
  165.     DDRD &= ~( (1<<DDD1)|(1<<DDD2) );
  166.     PORTD |= (1<<PD1)|(1<<PD2);
  167.  
  168.     /* Enable IO-pin interrupt */
  169.     PCICR |= (1<<PCIE2);
  170.     /* Unmask PD1 and SERVO_FEED_PIN */
  171.     PCMSK2 |= (1<<PCINT16) | (1<<PCINT17);
  172. #endif
  173.     /* Set output and turn off the servo current feed */
  174.     SERVO_FEED_DDR |= (1<<SERVO_FEED_PIN);
  175.     SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  176.  
  177.     txMsg.Id = (CAN_NMT_APP_START << CAN_SHIFT_NMT_TYPE) | (NODE_ID << CAN_SHIFT_NMT_SID);
  178.     txMsg.RemoteFlag = 0;
  179.     txMsg.ExtendedFlag = 1;
  180.     txMsg.DataLength = 4;
  181.     txMsg.Data.words[0] = APP_TYPE;
  182.     txMsg.Data.words[1] = APP_VERSION;
  183.  
  184.     BIOS_CanSend(&txMsg); // Send startup message
  185.  
  186.     txMsg.Id = 0x08000c0b; //(CLASS_SNS << CLASS_MASK_BITS)|(SNS_ACT_STATUS_SERVO << TYPE_MASK_BITS)|(NODE_ID << SID_MASK_BITS);
  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[0] == 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();
  238.  
  239.                             if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
  240.                                 blindsFailsafe();
  241.                             }else{
  242.                             txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  243.                             txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  244.                             txMsg.Data.bytes[2] = 0x00;
  245.                             txMsg.Data.bytes[3] = getPosition(1);
  246.                             txMsg.Data.bytes[4] = getPosition(2);
  247.                             txMsg.Data.bytes[5] = getPosition(3);
  248.                             txMsg.DataLength = 6;
  249.                             BIOS_CanSend(&txMsg);
  250.                         }*/
  251.                     }
  252.                 }
  253.             }
  254.  
  255.             rxMsgFull = 0;
  256.         }
  257.  
  258.         if( (Timebase_CurrentTime() - timeStamp)  >= STATUS_SEND_PERIOD ){
  259.             timeStamp = Timebase_CurrentTime();
  260.             // Send blinds status to CAN periodically
  261.  
  262.             boardTemperature = getTC1047temperature();
  263.  
  264.             if( (int32_t)boardTemperature >= FAILSAFE_TRESHOLD ){
  265.                 blindsFailsafe();
  266.                 }else{
  267.                 txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  268.                 txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  269.                 txMsg.Data.bytes[2] = 0x00;
  270.                 txMsg.Data.bytes[3] = getPosition(1);
  271.                 txMsg.Data.bytes[4] = getPosition(2);
  272.                 txMsg.Data.bytes[5] = getPosition(3);
  273.                 txMsg.DataLength = 6;
  274.  
  275.                 BIOS_CanSend(&txMsg);
  276.             }
  277.         }
  278.         // If start turning servo
  279.         if( turnDirection ){
  280.             if( Timebase_CurrentTime() - timeStampTurn >= TURN_PERIOD ){
  281.                 timeStampTurn = Timebase_CurrentTime();
  282.                 // Clockwise or counterclockwise?
  283.                 if( turnDirection == BLINDS_TURN_CCW ){
  284.                     alterPosition( -STEPS_PER_TURN_PERIOD , servoToTurn);
  285.                 }else if( turnDirection == BLINDS_TURN_CW ){
  286.                     alterPosition( STEPS_PER_TURN_PERIOD , servoToTurn);
  287.                 }
  288.                 servoFeed_timeStamp = Timebase_CurrentTime();
  289.             }
  290.         }
  291.  
  292.         // Turn off the servo current feed after timeout
  293.         if( Timebase_CurrentTime() - servoFeed_timeStamp >= SERVO_FEED_TIME ){
  294.             SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  295.             servoDisable();
  296.         }else{
  297.             SERVO_FEED_PORT |= (1<<SERVO_FEED_PIN);
  298.             servoEnable();
  299.         }
  300.     }
  301.     return 0;
  302. }
  303.  
  304. void blindsFailsafe()
  305. {
  306. /*  uint32_t boardTemperature = 0, timeStamp = 0;
  307.  
  308.     // Turn of servo current feed
  309.     SERVO_FEED_PORT &= ~(1<<SERVO_FEED_PIN);
  310.  
  311.  
  312.     while(1){
  313.         // check if any messages have been received
  314.         while(rxMsgFull) {
  315.             // This relay is adressed
  316.             if(rxMsg.Data.bytes[0] == BLINDS_CMD_RESET ){
  317.                 // Return from failsafe mode
  318.                 return;
  319.             }
  320.         }
  321.  
  322.         if( Timebase_CurrentTime() - timeStamp >= FAILSAFE_SEND_PERIOD ){
  323.             timeStamp = Timebase_CurrentTime();
  324.             // Send relay status to CAN
  325.             boardTemperature = getTC1047temperature();
  326.  
  327.             txMsg.Data.bytes[0] = boardTemperature & 0x00FF;
  328.             txMsg.Data.bytes[1] = (boardTemperature & 0xFF00)>>8;
  329.             txMsg.Data.bytes[2] = FAILSAFE_MODE;
  330. #if NUMBER_OF_RCS >0
  331.             txMsg.Data.bytes[3] = getPosition(1);
  332. #endif
  333. #if NUMBER_OF_RCS >1
  334.             txMsg.Data.bytes[4] = getPosition(2);
  335. #endif
  336. #if NUMBER_OF_RCS >2
  337.             txMsg.Data.bytes[5] = getPosition(3);
  338. #endif
  339.             txMsg.DataLength = NUMBER_OF_RCS+3;
  340.  
  341.             BIOS_CanSend(&txMsg);
  342.         }
  343.     }*/
  344. }
  345.  
  346.