Subversion Repositories HomeAutomation

Rev

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