Subversion Repositories HomeAutomation

Rev

Rev 1178 | Rev 1379 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1.  
  2. #include "act_PID_eeprom.h"
  3.  
  4. #include "act_PID.h"
  5. #include <drivers/misc/pid.h>
  6.  
  7. #define PID_ON 1
  8. #define PID_OFF 0
  9. #define PID_AUTO 2
  10.  
  11. //! Parameters for regulator
  12. struct PID_DATA pidData;
  13. struct PID_DEBUG_DATA pidDebugData;
  14.  
  15. uint8_t sensorModuleType, sensorModuleId,sensorId;
  16. uint8_t PID_Status;
  17. uint8_t calculatePID_flag,sendPID_flag = 0;
  18. uint16_t pwmValue=0;
  19. float referenceValue, measurementValue, inputValue;
  20.  
  21. struct eeprom_act_PID EEMEM eeprom_act_PID =
  22. {
  23.     {
  24.         ///TODO: Define initialization values on the EEPROM variables here, this will generate a *.eep file that can be used to store this values to the node, can in future be done with a EEPROM module and the make-scrips. Write the values in the exact same order as the struct is defined in the *.h file.
  25.         0x0200, // referenceValue
  26.         CAN_MODULE_TYPE_SNS_DS18X20,    //SensorModuleType
  27.         0x00,   //SensorModuleId
  28.         0x00,   //SensorId
  29.         0x0000, //uint16_t K_P;
  30.         0x0000, //uint16_t K_I;
  31.         0x0000, //uint16_t K_D;
  32.         0x00,   //uint8_t TimeMsOrS;
  33.         0x0000, //uint16_t Time;
  34.         CAN_MODULE_TYPE_ACT_SOFTPWM,    //ActuatorModuleType
  35.         0x00,   //ActuatorModuleId
  36.         0x00,   //ActuatorId
  37.         10, //SendPeriod
  38.  
  39.     },
  40.     0   // crc, must be a correct value, but this will also be handled by the EEPROM module or make scripts
  41. };
  42.  
  43. void calculatePID(void) {
  44.     if (PID_Status == PID_OFF){
  45.         ;// use current PWMvalue
  46.     } else {
  47.         pwmValue += (int16_t) pid_Controller(referenceValue, measurementValue, &pidData, &pidDebugData);
  48.         printf("InOut: ref: %d, meas: %d\n",(int16_t) (referenceValue*10), (int16_t) (measurementValue*10));
  49.         if (pwmValue < MIN_PWM_VALUE) {
  50.             pwmValue = MIN_PWM_VALUE;
  51.         }
  52.         else if (pwmValue > MAX_PWM_VALUE) {
  53.             pwmValue = MAX_PWM_VALUE-1;
  54.         }
  55.         //send current PWM value as soon as possible
  56.         sendPID_flag=1;
  57.     }
  58. printf("PWM: %d\n",pwmValue);
  59. }
  60.  
  61.  
  62. void calculatePID_callback(uint8_t timer)
  63. {
  64.  
  65.     if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_SECONDS) {
  66.         static uint8_t seconds = 0;
  67.         seconds++;
  68.         if (seconds >= eeprom_read_word(EEDATA16.Time))
  69.         {
  70.             seconds = 0;
  71.             calculatePID_flag = 1;
  72.         }
  73.     } else {
  74.         calculatePID_flag = 1;
  75.     }
  76. }
  77.  
  78. void sendPID(void)
  79. {
  80.     if (eeprom_read_byte(EEDATA.actuatorModuleType) != 0) {
  81.         StdCan_Msg_t txMsg;
  82.         StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  83.         StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_TO_OWNER);
  84.         txMsg.Header.ModuleType = eeprom_read_byte(EEDATA.actuatorModuleType); ///TODO: Change this to the actual module type
  85.         txMsg.Header.ModuleId = eeprom_read_byte(EEDATA.actuatorModuleId);
  86.         txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PWM;
  87.         txMsg.Length = 3;
  88.         txMsg.Data[0] = eeprom_read_byte(EEDATA.actuatorId);
  89.         int16_t tempPWM =(int16_t) (((int32_t)pwmValue)*64/100);
  90.         txMsg.Data[1] = ( (tempPWM)>>8)&0xff;
  91.         txMsg.Data[2] = ( (tempPWM))&0xff;
  92.         while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  93.     }
  94.     StdCan_Msg_t txMsg;
  95.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  96.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  97.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
  98.     txMsg.Header.ModuleId = act_PID_ID;
  99.     txMsg.Header.Command = CAN_MODULE_CMD_PID_PID_STATUS;
  100.     txMsg.Length = 8;
  101.     txMsg.Data[0] = (uint8_t)0x00ff & (((uint32_t)(measurementValue*64))>>8);
  102.     txMsg.Data[1] = (uint8_t)0x00ff & ((uint32_t)measurementValue*64);
  103.     txMsg.Data[2] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
  104.     txMsg.Data[3] = (uint8_t)0x00ff & ((uint32_t)referenceValue*64);
  105.     int16_t tempPWM =(int16_t) (((int32_t)pwmValue)*64/100);
  106.     txMsg.Data[4] = ( (tempPWM)>>8)&0xff;
  107.     txMsg.Data[5] = ( (tempPWM))&0xff;
  108.     txMsg.Data[6] = ((int16_t) (pidDebugData.Sum)>>8)&0xff;
  109.     txMsg.Data[7] = ((int16_t) (pidDebugData.Sum))&0xff;
  110.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  111. }
  112.  
  113. void sendPID_callback(uint8_t timer) {
  114.   sendPID_flag=1;
  115. }
  116.  
  117. #ifdef act_PID_SEND_DEBUG_TIMER
  118. void sendPID_debug_callback(uint8_t timer)
  119. {
  120.     StdCan_Msg_t txMsg;
  121.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  122.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  123.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
  124.     txMsg.Header.ModuleId = act_PID_ID;
  125.     txMsg.Header.Command = CAN_MODULE_CMD_PID_DEBUG;
  126.     txMsg.Length = 8;
  127.  
  128.     txMsg.Data[0] = ((int16_t) (pidDebugData.P_term)>>8)&0xff;
  129.     txMsg.Data[1] = ((int16_t) (pidDebugData.P_term))&0xff;
  130.     txMsg.Data[2] = ((int16_t) (pidDebugData.I_term)>>8)&0xff;
  131.     txMsg.Data[3] = ((int16_t) (pidDebugData.I_term))&0xff;
  132.     txMsg.Data[4] = ((int16_t) (pidDebugData.D_term)>>8)&0xff;
  133.     txMsg.Data[5] = ((int16_t) (pidDebugData.D_term))&0xff;
  134.     txMsg.Data[6] = ((int16_t) (pidDebugData.Sum)>>8)&0xff;
  135.     txMsg.Data[7] = ((int16_t) (pidDebugData.Sum))&0xff;
  136.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  137.     printf("PID: P:%d, I:%d, D:%d, S:%d\n",(int16_t)pidDebugData.P_term,(int16_t)pidDebugData.I_term,(int16_t)pidDebugData.D_term,(int16_t)pidDebugData.Sum);
  138. }
  139. #endif
  140.  
  141. void act_PID_Init(void)
  142. {
  143.     if (EEDATA_OK)
  144.     {
  145.     } else
  146.     {   //The CRC of the EEPROM is not correct, store default values and update CRC
  147.       eeprom_write_word_crc(EEDATA16.referenceValue, 0x0520, WITHOUT_CRC);
  148.       eeprom_write_byte_crc(EEDATA.sensorModuleType, PID_TEMPERATURE_SENSOR_MODULE_TYPE, WITHOUT_CRC);
  149.       eeprom_write_byte_crc(EEDATA.sensorModuleId, PID_TEMPERATURE_SENSOR_MODULE_ID, WITHOUT_CRC);
  150.       eeprom_write_byte_crc(EEDATA.sensorId, PID_TEMPERATURE_SENSOR, WITHOUT_CRC);
  151.       eeprom_write_word_crc(EEDATA16.K_P, (int16_t)(DEFAULT_K_P*64), WITHOUT_CRC);
  152.       eeprom_write_word_crc(EEDATA16.K_I, (int16_t)(DEFAULT_K_I*64), WITHOUT_CRC);
  153.       eeprom_write_word_crc(EEDATA16.K_D, (int16_t)(DEFAULT_K_D*64), WITHOUT_CRC);
  154.       eeprom_write_byte_crc(EEDATA.TimeMsOrS, DEFAULT_PID_CALC_PERIOD_UNIT, WITHOUT_CRC);
  155.       eeprom_write_word_crc(EEDATA16.Time, DEFAULT_PID_CALC_PERIOD, WITHOUT_CRC);
  156.       eeprom_write_byte_crc(EEDATA.actuatorModuleType, PID_PWM_ACTUATOR_MODULE_TYPE, WITHOUT_CRC);
  157.       eeprom_write_byte_crc(EEDATA.actuatorModuleId, PID_PWM_ACTUATOR_MODULE_ID, WITHOUT_CRC);
  158.       eeprom_write_byte_crc(EEDATA.actuatorId, PID_PWM_ACTUATOR, WITHOUT_CRC);
  159.       eeprom_write_byte_crc(EEDATA.sendPeriod, 10, WITH_CRC);
  160.     }
  161.     referenceValue = (float) eeprom_read_word(EEDATA16.referenceValue)/64;
  162.     sensorModuleType = eeprom_read_byte(EEDATA.sensorModuleType);
  163.     sensorModuleId = eeprom_read_byte(EEDATA.sensorModuleId);
  164.     sensorId = eeprom_read_byte(EEDATA.sensorId);
  165.  
  166.  
  167.     if (eeprom_read_byte(EEDATA.TimeMsOrS) == CAN_MODULE_ENUM_PID_CONFIG_PARAMETER_TIMEUNIT_SECONDS) {
  168.         Timer_SetTimeout(act_PID_TIMER, 1000, TimerTypeFreeRunning, &calculatePID_callback);
  169.     } else {
  170.         Timer_SetTimeout(act_PID_TIMER, eeprom_read_word(EEDATA16.Time), TimerTypeFreeRunning, &calculatePID_callback);
  171.     }
  172.  
  173.     pid_Init((float) eeprom_read_word(EEDATA16.K_P)/64 * SCALING_FACTOR, (float) eeprom_read_word(EEDATA16.K_I)/64 * SCALING_FACTOR , (float) eeprom_read_word(EEDATA16.K_D)/64 * SCALING_FACTOR , &pidData);
  174.     pwmValue += (int16_t) pid_Controller(referenceValue, measurementValue, &pidData, &pidDebugData);
  175.     PID_Status = PID_ON;
  176.     pwmValue = DEFAULT_PWM_VALUE;
  177.     Timer_SetTimeout(act_PID_SEND_TIMER, eeprom_read_byte(EEDATA.sendPeriod)*1000, TimerTypeFreeRunning, &sendPID_callback);
  178.     #ifdef act_PID_SEND_DEBUG_TIMER
  179.       Timer_SetTimeout(act_PID_SEND_DEBUG_TIMER, PID_SEND_DEBUG_PERIOD, TimerTypeFreeRunning, &sendPID_debug_callback);
  180.     #endif
  181. }
  182.  
  183. void act_PID_Process(void)
  184. {
  185.     if (calculatePID_flag) {
  186.         calculatePID();
  187.         calculatePID_flag = 0;
  188.     }
  189.     if (sendPID_flag) {
  190.         sendPID();
  191.         sendPID_flag= 0;
  192.     }
  193. }
  194.  
  195. void act_PID_HandleMessage(StdCan_Msg_t *rxMsg)
  196. {
  197.  
  198.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_ACT &&
  199.             StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_TO_OWNER &&
  200.             rxMsg->Header.ModuleType == CAN_MODULE_TYPE_ACT_PID &&
  201.             rxMsg->Header.ModuleId == act_PID_ID)
  202.     {
  203.         switch (rxMsg->Header.Command)
  204.         {
  205.         case CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS:
  206.             if (rxMsg->Data[0]==0)  //sensor id shall be zero
  207.             {
  208.                 if (rxMsg->Length == 3)
  209.                 {
  210.                     if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2])
  211.                     {
  212.                         pid_Reset_Integrator(&pidData);
  213.                         PID_Status = PID_AUTO;
  214.                     }
  215.                     else
  216.                     {
  217.                         pid_Reset_Integrator(&pidData);
  218.                         eeprom_write_word_crc(EEDATA16.referenceValue, ((rxMsg->Data[1]<<8) + rxMsg->Data[2]), WITH_CRC);
  219.                         referenceValue = ((rxMsg->Data[1]<<8) + rxMsg->Data[2])/64;
  220.                     }
  221.                 } else
  222.                 {
  223.                     rxMsg->Data[1] = (uint8_t)0x00ff & (((uint32_t)(referenceValue*64))>>8);
  224.                     rxMsg->Data[2] = (uint8_t)0x00ff & ((uint32_t)referenceValue*64);
  225.                     StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  226.                     rxMsg->Length = 3;
  227.                     while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  228.                 }
  229.  
  230.             }
  231.         break;
  232.         case CAN_MODULE_CMD_PID_CONFIG_SENSOR:
  233.             if (rxMsg->Length == 3)
  234.             {
  235.                 eeprom_write_byte_crc(EEDATA.sensorModuleType, rxMsg->Data[0] , WITHOUT_CRC);
  236.                 eeprom_write_byte_crc(EEDATA.sensorModuleId, rxMsg->Data[1] , WITHOUT_CRC);
  237.                 eeprom_write_byte_crc(EEDATA.sensorId, rxMsg->Data[2] , WITH_CRC);
  238.                 sensorModuleType = eeprom_read_byte(EEDATA.sensorModuleType);
  239.                 sensorModuleId = eeprom_read_byte(EEDATA.sensorModuleId);
  240.                 sensorId = eeprom_read_byte(EEDATA.sensorId);
  241.             }
  242.             rxMsg->Data[0] = eeprom_read_byte(EEDATA.sensorModuleType);
  243.             rxMsg->Data[1] = eeprom_read_byte(EEDATA.sensorModuleId);
  244.             rxMsg->Data[2] = eeprom_read_byte(EEDATA.sensorId);
  245.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  246.             rxMsg->Length = 3;
  247.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  248.         break;
  249.         case CAN_MODULE_CMD_PID_CONFIG_ACTUATOR:
  250.             if (rxMsg->Length == 3)
  251.             {
  252.                 eeprom_write_byte_crc(EEDATA.actuatorModuleType, rxMsg->Data[0] , WITHOUT_CRC);
  253.                 eeprom_write_byte_crc(EEDATA.actuatorModuleId, rxMsg->Data[1] , WITHOUT_CRC);
  254.                 eeprom_write_byte_crc(EEDATA.actuatorId, rxMsg->Data[2] , WITH_CRC);
  255.             }
  256.             rxMsg->Data[0] = eeprom_read_byte(EEDATA.actuatorModuleType);
  257.             rxMsg->Data[1] = eeprom_read_byte(EEDATA.actuatorModuleId);
  258.             rxMsg->Data[2] = eeprom_read_byte(EEDATA.actuatorId);
  259.             StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  260.             rxMsg->Length = 3;
  261.             while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  262.         break;
  263.         case CAN_MODULE_CMD_GLOBAL_REPORT_INTERVAL:
  264.             if (rxMsg->Length == 1) {
  265.                 if (65 <= rxMsg->Data[0])
  266.                     rxMsg->Data[0]=65;
  267.                 if (0 == rxMsg->Data[0])
  268.                     rxMsg->Data[0]=1;
  269.                 eeprom_write_byte_crc(EEDATA.sendPeriod, rxMsg->Data[0], WITH_CRC);
  270.                 Timer_SetTimeout(act_PID_SEND_TIMER, rxMsg->Data[0]*1000, TimerTypeFreeRunning, &sendPID_callback);
  271.                 StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  272.                 while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  273.             }
  274.         break;
  275.  
  276.         case CAN_MODULE_CMD_PID_CONFIG_PARAMETER:
  277.             if (rxMsg->Length == 8)
  278.             {
  279.                 eeprom_write_word_crc(EEDATA16.K_P, rxMsg->Data[1]+(rxMsg->Data[0]<<8), WITHOUT_CRC);
  280.                 eeprom_write_word_crc(EEDATA16.K_I, rxMsg->Data[3]+(rxMsg->Data[2]<<8), WITHOUT_CRC);
  281.                 eeprom_write_word_crc(EEDATA16.K_D, rxMsg->Data[5]+(rxMsg->Data[4]<<8), WITHOUT_CRC);
  282.                 eeprom_write_byte_crc(EEDATA.TimeMsOrS, ((rxMsg->Data[6]&0x80)>>7), WITHOUT_CRC);
  283.                 eeprom_write_word_crc(EEDATA16.Time, rxMsg->Data[7]+((rxMsg->Data[6]&0x7f)<<8), WITH_CRC);
  284.                 if (eeprom_read_byte(EEDATA.TimeMsOrS) == 0) {
  285.                     Timer_SetTimeout(act_PID_TIMER, 1000, TimerTypeFreeRunning, &calculatePID_callback);
  286.                 } else {
  287.                     Timer_SetTimeout(act_PID_TIMER, eeprom_read_word(EEDATA16.Time), TimerTypeFreeRunning, &calculatePID_callback);
  288.                 }
  289.  
  290.                 pid_Init((float) eeprom_read_word(EEDATA16.K_P)/64 * SCALING_FACTOR, (float) eeprom_read_word(EEDATA16.K_I)/64 * SCALING_FACTOR , (float) eeprom_read_word(EEDATA16.K_D)/64 * SCALING_FACTOR , &pidData);
  291.                 pwmValue += (int16_t) pid_Controller(referenceValue, measurementValue, &pidData, &pidDebugData);
  292.                 PID_Status = PID_ON;
  293.                 pwmValue = DEFAULT_PWM_VALUE;
  294.             } else
  295.             {
  296.                 rxMsg->Data[0] = (0xff00&(eeprom_read_word(EEDATA16.K_P))>>8);
  297.                 rxMsg->Data[1] = (0x00ff&(eeprom_read_word(EEDATA16.K_P)));
  298.                 rxMsg->Data[2] = (0xff00&(eeprom_read_word(EEDATA16.K_I))>>8);
  299.                 rxMsg->Data[3] = (0x00ff&(eeprom_read_word(EEDATA16.K_I)));
  300.                 rxMsg->Data[4] = (0xff00&(eeprom_read_word(EEDATA16.K_D))>>8);
  301.                 rxMsg->Data[5] = (0x00ff&(eeprom_read_word(EEDATA16.K_D)));
  302.                 rxMsg->Data[6] = (0xff00&(eeprom_read_word(EEDATA16.Time))>>8);
  303.                 rxMsg->Data[7] = (0x00ff&(eeprom_read_word(EEDATA16.Time)));
  304.                 rxMsg->Data[6] |= (0x80&(eeprom_read_byte(EEDATA.TimeMsOrS))<<7);
  305.                 StdCan_Set_direction(rxMsg->Header, DIRECTIONFLAG_FROM_OWNER);
  306.                 rxMsg->Length = 8;
  307.                 while (StdCan_Put(rxMsg) != StdCan_Ret_OK);
  308.             }
  309.         break;
  310.         }
  311.     }
  312.  
  313.  
  314.     if (    StdCan_Ret_class(rxMsg->Header) == CAN_MODULE_CLASS_SNS &&
  315.                 StdCan_Ret_direction(rxMsg->Header) == DIRECTIONFLAG_FROM_OWNER &&
  316.                 rxMsg->Header.ModuleType == sensorModuleType &&
  317.                 rxMsg->Header.ModuleId == sensorModuleId &&
  318.                 rxMsg->Header.Command == CAN_MODULE_CMD_PHYSICAL_TEMPERATURE_CELSIUS &&
  319.                 rxMsg->Data[0] == sensorId)
  320.         {
  321.             if (0x80 == rxMsg->Data[1] && 0x00 == rxMsg->Data[2])
  322.             {
  323.                 //Error on the temperature signal, do something
  324.             }
  325.             else
  326.             {
  327.                 measurementValue = ((float)((rxMsg->Data[1]<<8) + rxMsg->Data[2]))/64;
  328.             }
  329.  
  330.         }
  331.  
  332. }
  333.  
  334. void act_PID_List(uint8_t ModuleSequenceNumber)
  335. {
  336.     StdCan_Msg_t txMsg;
  337.    
  338.     StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_ACT); ///TODO: Change this to the actual class type
  339.     StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
  340.     txMsg.Header.ModuleType = CAN_MODULE_TYPE_ACT_PID; ///TODO: Change this to the actual module type
  341.     txMsg.Header.ModuleId = act_PID_ID;
  342.     txMsg.Header.Command = CAN_MODULE_CMD_GLOBAL_LIST;
  343.     txMsg.Length = 6;
  344.  
  345.     txMsg.Data[0] = NODE_HW_ID_BYTE0;
  346.     txMsg.Data[1] = NODE_HW_ID_BYTE1;
  347.     txMsg.Data[2] = NODE_HW_ID_BYTE2;
  348.     txMsg.Data[3] = NODE_HW_ID_BYTE3;
  349.    
  350.     txMsg.Data[4] = NUMBER_OF_MODULES;
  351.     txMsg.Data[5] = ModuleSequenceNumber;
  352.    
  353.     while (StdCan_Put(&txMsg) != StdCan_Ret_OK);
  354. }
  355.