void vnh_standby(vnh_s* v){
vnh_disable(v);
gpio_clr_pin(v->mota);
gpio_clr_pin(v->motb);
// Set mode
v->mode = VNH_STANDBY;
}
void st7564r_hard_reset(void)
{
gpio_clr_pin(LCD.a0);
gpio_clr_pin(LCD.reset);
_delay_us(5);
gpio_set_pin(LCD.reset);
_delay_us(5);
}
void vnh_disable(vnh_s* v){
// Set enable pins as outputs
gpio_set_ddr(v->ena);
gpio_set_ddr(v->enb);
// Disable outputs
gpio_clr_pin(v->ena);
gpio_clr_pin(v->enb);
}
void st7565r_write_command(unsigned char cmd)
{
gpio_clr_pin(LCD.a0);
gpio_clr_pin(LCD.chip_select);
bitbang_spi_write(LCD.data, LCD.clk, cmd);
gpio_set_pin(LCD.chip_select);
}
void linearAct_stop(void) {
gpio_clr_pin(RELAY_ON);
_delay_ms(10);
gpio_clr_pin(RELAY_DIR);
stopfactor = 1;
// start stop-timer
Timer_SetTimeout(act_linearAct_TIMER_stop, stopTimeout , TimerTypeOneShot, linearAct_cleanUp);
//printf("In linearAct_stop\n");
}
void act_linearAct_Init(void)
{
#if act_linearAct_USEEEPROM==1
if (EEDATA_OK)
{
///TODO: Use stored data to set initial values for the module
pulses = eeprom_read_word(EEDATA16.pulses_ee);
min = eeprom_read_word(EEDATA16.min_ee);
low = eeprom_read_word(EEDATA16.low_ee);
high = eeprom_read_word(EEDATA16.high_ee);
max = eeprom_read_word(EEDATA16.max_ee);
} else
{ //The CRC of the EEPROM is not correct, store default values and update CRC
eeprom_write_word_crc(EEDATA16.pulses_ee, 1200, WITHOUT_CRC);
eeprom_write_word_crc(EEDATA16.min_ee, 300, WITHOUT_CRC);
eeprom_write_word_crc(EEDATA16.low_ee, 815, WITHOUT_CRC);
eeprom_write_word_crc(EEDATA16.high_ee, 5600, WITHOUT_CRC);
eeprom_write_word_crc(EEDATA16.max_ee, 6100, WITHOUT_CRC);
EEDATA_UPDATE_CRC;
}
#else
// Only for testing/debugging...
pulses = 1200;
min = 300;
low = 815;
high = 5600;
max = 6100;
#endif
// PD5 (EXP_G) is used as Timer1 input. Hardwired, no move possible!
gpio_set_in(EXP_G);
gpio_set_pullup(EXP_G);
//PORTD &= ~(_BV(DIR_RELAY) | _BV(ON_RELAY));
gpio_clr_pin(RELAY_ON);
gpio_clr_pin(RELAY_DIR);
// initiera utgångar
//DDRD = (_BV(DIR_RELAY) | _BV(ON_RELAY));
gpio_set_out(RELAY_ON);
gpio_set_out(RELAY_DIR);
// Start report timer
Timer_SetTimeout(act_linearAct_TIMER_report, act_linearAct_ReportInterval*1000 , TimerTypeFreeRunning, 0);
#if (USE_STIMULI == 1)
// Set stimuli as an output
gpio_set_out(STIMULI);
Timer_SetTimeout(act_linearAct_TIMER_stimuli, 10, TimerTypeFreeRunning,0);
#endif
}
void vnh_reset(vnh_s* v){
// Set pins for switches as outputs
gpio_set_ddr(v->mota);
gpio_set_ddr(v->motb);
// Disable outputs
vnh_disable(v);
// Toggle motor outputs to clear any faults
gpio_set_pin(v->mota);
gpio_set_pin(v->motb);
gpio_clr_pin(v->mota);
gpio_clr_pin(v->motb);
// Set mode
v->mode = VNH_STANDBY;
}
void st7565r_write_data(unsigned char data)
{
gpio_set_pin(LCD.a0);
gpio_clr_pin(LCD.chip_select);
bitbang_spi_write(LCD.data, LCD.clk, data);
gpio_set_pin(LCD.chip_select);
}
void vnh_reverse(vnh_s* v){
vnh_disable(v); // Disable outputs
// A=lo, B=hi
gpio_clr_pin(v->mota);
gpio_set_pin(v->motb);
vnh_enable(v); // Enable outputs
// Set status
v->mode = VNH_REVERSE;
}
void vnh_forward(vnh_s* v){
vnh_disable(v); // Disable outputs
// A=hi, B=lo
gpio_set_pin(v->mota);
gpio_clr_pin(v->motb);
vnh_enable(v); // Enable outputs
// Set status
v->mode = VNH_FORWARD;
}
/**
Move to position
Position is a 16 bits integer between min and max.
Not degrees, nor percent.
*/
uint8_t linearAct_setPosition (uint16_t new_position) {
/* Check if position is within limits */
if (new_position > max) new_position = max;
if (new_position < min) new_position = min;
// First check if we need to move at all
if (new_position != pulses) {
// Check if we already are in movement, then exit with value 1
if (direction != 0) {
//stop();
//while (in_motion());
return 1; // no autostop, let the user stop movement first. This may be improved.
}
if (new_position > pulses) {
if (latest_stop_pulses_up > (new_position - pulses)) return 1; // We will move past target, so don't move
OCR1A = new_position - pulses - latest_stop_pulses_up; // set Output Compare target
TIMSK1 = _BV(OCIE1A); // activate interrupts for output compare!
direction = UP; // Adjust direction for software
gpio_set_pin(RELAY_DIR); // Adjust direction physically
}
else {
if (latest_stop_pulses_up > (pulses - new_position)) return 1; // We will move past target, so don't move
OCR1A = pulses - new_position - latest_stop_pulses_down;
TIMSK1 = _BV(OCIE1A);
direction = DOWN;
gpio_clr_pin(RELAY_DIR);
}
TCCR1B = _BV(CS12) | _BV(CS11); // Activate counter
_delay_ms(10); // wait until direction relay is ready
gpio_clr_pin(RELAY_ON); // Start moving!
}
return 0;
}
/* Will set up pullup enable outputs according config */
void setPullups(void)
{
if (sns_inputAnalog_Config[0].PullupEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_PULLUP_ENABLE)
{
#if sns_inputAnalog0PullupPCA95xx==0
gpio_set_pin(sns_inputAnalog0Pullup);
gpio_set_out(sns_inputAnalog0Pullup);
#else
Pca95xx_set_pin(sns_inputAnalog0Pullup);
Pca95xx_set_out(sns_inputAnalog0Pullup);
#endif
}
else
{
#if sns_inputAnalog0PullupPCA95xx==0
gpio_clr_pin(sns_inputAnalog0Pullup);
gpio_set_out(sns_inputAnalog0Pullup);
#else
Pca95xx_clr_pin(sns_inputAnalog0Pullup);
Pca95xx_set_out(sns_inputAnalog0Pullup);
#endif
}
#if sns_inputAnalog_NUM_SUPPORTED>=2
if (sns_inputAnalog_Config[1].PullupEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_PULLUP_ENABLE)
{
#if sns_inputAnalog1PullupPCA95xx==0
gpio_set_pin(sns_inputAnalog1Pullup);
gpio_set_out(sns_inputAnalog1Pullup);
#else
Pca95xx_set_pin(sns_inputAnalog1Pullup);
Pca95xx_set_out(sns_inputAnalog1Pullup);
#endif
}
else
{
#if sns_inputAnalog1PullupPCA95xx==0
gpio_clr_pin(sns_inputAnalog1Pullup);
gpio_set_out(sns_inputAnalog1Pullup);
#else
Pca95xx_clr_pin(sns_inputAnalog1Pullup);
Pca95xx_set_out(sns_inputAnalog1Pullup);
#endif
}
#endif
#if sns_inputAnalog_NUM_SUPPORTED>=3
if (sns_inputAnalog_Config[2].PullupEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_PULLUP_ENABLE)
{
#if sns_inputAnalog2PullupPCA95xx==0
gpio_set_pin(sns_inputAnalog2Pullup);
gpio_set_out(sns_inputAnalog2Pullup);
#else
Pca95xx_set_pin(sns_inputAnalog2Pullup);
Pca95xx_set_out(sns_inputAnalog2Pullup);
#endif
}
else
{
#if sns_inputAnalog2PullupPCA95xx==0
gpio_clr_pin(sns_inputAnalog2Pullup);
gpio_set_out(sns_inputAnalog2Pullup);
#else
Pca95xx_clr_pin(sns_inputAnalog2Pullup);
Pca95xx_set_out(sns_inputAnalog2Pullup);
#endif
}
#endif
#if sns_inputAnalog_NUM_SUPPORTED>=4
if (sns_inputAnalog_Config[3].PullupEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_PULLUP_ENABLE)
{
#if sns_inputAnalog3PullupPCA95xx==0
gpio_set_pin(sns_inputAnalog3Pullup);
gpio_set_out(sns_inputAnalog3Pullup);
#else
Pca95xx_set_pin(sns_inputAnalog3Pullup);
Pca95xx_set_out(sns_inputAnalog3Pullup);
#endif
}
else
{
#if sns_inputAnalog3PullupPCA95xx==0
gpio_clr_pin(sns_inputAnalog3Pullup);
gpio_set_out(sns_inputAnalog3Pullup);
#else
Pca95xx_clr_pin(sns_inputAnalog3Pullup);
Pca95xx_set_out(sns_inputAnalog3Pullup);
#endif
}
#endif
}
/* Will set up reference enable outputs according config */
void setReferences(void)
{
if (sns_inputAnalog_Config[0].RefEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_REFERENCE_ENABLE)
{
#if sns_inputAnalog0RefPCA95xx==0
gpio_set_pin(sns_inputAnalog0Ref);
gpio_set_out(sns_inputAnalog0Ref);
#else
Pca95xx_set_pin(sns_inputAnalog0Ref);
Pca95xx_set_out(sns_inputAnalog0Ref);
#endif
}
else
{
#if sns_inputAnalog0RefPCA95xx==0
gpio_clr_pin(sns_inputAnalog0Ref);
gpio_set_out(sns_inputAnalog0Ref);
#else
Pca95xx_clr_pin(sns_inputAnalog0Ref);
Pca95xx_set_out(sns_inputAnalog0Ref);
#endif
}
#if sns_inputAnalog_NUM_SUPPORTED>=2
if (sns_inputAnalog_Config[1].RefEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_REFERENCE_ENABLE)
{
#if sns_inputAnalog1RefPCA95xx==0
gpio_set_pin(sns_inputAnalog1Ref);
gpio_set_out(sns_inputAnalog1Ref);
#else
Pca95xx_set_pin(sns_inputAnalog1Ref);
Pca95xx_set_out(sns_inputAnalog1Ref);
#endif
}
else
{
#if sns_inputAnalog1RefPCA95xx==0
gpio_clr_pin(sns_inputAnalog1Ref);
gpio_set_out(sns_inputAnalog1Ref);
#else
Pca95xx_clr_pin(sns_inputAnalog1Ref);
Pca95xx_set_out(sns_inputAnalog1Ref);
#endif
}
#endif
#if sns_inputAnalog_NUM_SUPPORTED>=3
if (sns_inputAnalog_Config[2].RefEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_REFERENCE_ENABLE)
{
#if sns_inputAnalog2RefPCA95xx==0
gpio_set_pin(sns_inputAnalog2Ref);
gpio_set_out(sns_inputAnalog2Ref);
#else
Pca95xx_set_pin(sns_inputAnalog2Ref);
Pca95xx_set_out(sns_inputAnalog2Ref);
#endif
}
else
{
#if sns_inputAnalog2RefPCA95xx==0
gpio_clr_pin(sns_inputAnalog2Ref);
gpio_set_out(sns_inputAnalog2Ref);
#else
Pca95xx_clr_pin(sns_inputAnalog2Ref);
Pca95xx_set_out(sns_inputAnalog2Ref);
#endif
}
#endif
#if sns_inputAnalog_NUM_SUPPORTED>=4
if (sns_inputAnalog_Config[3].RefEnable==CAN_MODULE_ENUM_INPUTANALOG_ANALOGCONFIG_REFERENCE_ENABLE)
{
#if sns_inputAnalog3RefPCA95xx==0
gpio_set_pin(sns_inputAnalog3Ref);
gpio_set_out(sns_inputAnalog3Ref);
#else
Pca95xx_set_pin(sns_inputAnalog3Ref);
Pca95xx_set_out(sns_inputAnalog3Ref);
#endif
}
else
{
#if sns_inputAnalog3RefPCA95xx==0
gpio_clr_pin(sns_inputAnalog3Ref);
gpio_set_out(sns_inputAnalog3Ref);
#else
Pca95xx_clr_pin(sns_inputAnalog3Ref);
Pca95xx_set_out(sns_inputAnalog3Ref);
#endif
}
#endif
}
void sns_flower_Process(void)
{
static uint8_t measureState=0;
static uint16_t results[6];
static uint16_t resultsInv[6];
uint16_t temp;
static uint8_t flowerChannelToSend = 0;
if (Timer_Expired(sns_flower_WAIT_TIMER)) {
gpio_set_out(sns_flower_highSide_PIN);
gpio_set_out(sns_flower_lowSide_PIN);
gpio_set_pin(sns_flower_highSide_PIN);
gpio_clr_pin(sns_flower_lowSide_PIN);
measureState=0;
Timer_SetTimeout(sns_flower_MEASUREMENT_TIMER, sns_flower_MEASUREMENT_PERIOD_MS , TimerTypeOneShot, 0);
}
if (Timer_Expired(sns_flower_MEASUREMENT_TIMER)) {
StdCan_Msg_t txMsg;
StdCan_Set_class(txMsg.Header, CAN_MODULE_CLASS_SNS);
StdCan_Set_direction(txMsg.Header, DIRECTIONFLAG_FROM_OWNER);
txMsg.Header.ModuleType = CAN_MODULE_TYPE_SNS_VOLTAGECURRENT;
txMsg.Header.ModuleId = sns_flower_ID;
txMsg.Length = 3;
switch (measureState) {
case 0:
#ifdef sns_flower0AD
results[0] = ADC_Get(sns_flower0AD);
#endif
#ifdef sns_flower1AD
results[1] = ADC_Get(sns_flower1AD);
#endif
#ifdef sns_flower2AD
results[2] = ADC_Get(sns_flower2AD);
#endif
#ifdef sns_flower3AD
results[3] = ADC_Get(sns_flower3AD);
#endif
#ifdef sns_flower4AD
results[4] = ADC_Get(sns_flower4AD);
#endif
#ifdef sns_flower5AD
results[5] = ADC_Get(sns_flower5AD);
#endif
gpio_clr_pin(sns_flower_highSide_PIN);
gpio_set_pin(sns_flower_lowSide_PIN);
measureState=1;
Timer_SetTimeout(sns_flower_MEASUREMENT_TIMER, sns_flower_MEASUREMENT_PERIOD_MS , TimerTypeOneShot, 0);
break;
case 1:
#ifdef sns_flower0AD
resultsInv[0] = 1023-ADC_Get(sns_flower0AD);
#endif
#ifdef sns_flower1AD
resultsInv[1] = 1023-ADC_Get(sns_flower1AD);
#endif
#ifdef sns_flower2AD
resultsInv[2] = 1023-ADC_Get(sns_flower2AD);
#endif
#ifdef sns_flower3AD
resultsInv[3] = 1023-ADC_Get(sns_flower3AD);
#endif
#ifdef sns_flower4AD
resultsInv[4] = 1023-ADC_Get(sns_flower4AD);
#endif
#ifdef sns_flower5AD
resultsInv[5] = 1023-ADC_Get(sns_flower5AD);
#endif
gpio_clr_pin(sns_flower_highSide_PIN);
gpio_clr_pin(sns_flower_lowSide_PIN);
gpio_set_in(sns_flower_highSide_PIN);
gpio_set_in(sns_flower_lowSide_PIN);
measureState=2;
flowerChannelToSend=0;
Timer_SetTimeout(sns_flower_MEASUREMENT_TIMER, sns_flower_MEASUREMENT_PERIOD_MS , TimerTypeOneShot, 0);
break;
case 2:
switch(flowerChannelToSend) {
case 0:
#ifdef sns_flower0AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 0;
temp = (uint16_t)((float)(results[0]+resultsInv[0])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 1;
# if !(defined(sns_flower1AD) || defined(sns_flower2AD) || defined(sns_flower3AD) || defined(sns_flower4AD) || defined(sns_flower5AD))
flowerChannelToSend = 0;
measureState=0;
# endif
break;
#endif
case 1:
#ifdef sns_flower1AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 1;
temp = (uint16_t)((float)(results[1]+resultsInv[1])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 2;
# if !(defined(sns_flower2AD) || defined(sns_flower3AD) || defined(sns_flower4AD) || defined(sns_flower5AD))
flowerChannelToSend = 0;
measureState=0;
# endif
break;
#endif
case 2:
#ifdef sns_flower2AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 2;
temp = (uint16_t)((float)(results[2]+resultsInv[2])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 3;
#if !(defined(sns_flower3AD) || defined(sns_flower4AD) || defined(sns_flower5AD))
flowerChannelToSend = 0;
measureState=0;
#endif
break;
#endif
case 3:
#ifdef sns_flower3AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 3;
temp = (uint16_t)((float)(results[3]+resultsInv[3])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 4;
#if !( defined(sns_flower4AD) || defined(sns_flower5AD))
flowerChannelToSend = 0;
measureState=0;
#endif
break;
#endif
case 4:
#ifdef sns_flower4AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 4;
temp = (uint16_t)((float)(results[4]+resultsInv[4])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 5;
#if !(defined(sns_flower5AD))
flowerChannelToSend = 0;
measureState=0;
#endif
break;
#endif
case 5:
#ifdef sns_flower5AD
txMsg.Header.Command = CAN_MODULE_CMD_PHYSICAL_PERCENT;
txMsg.Data[0] = 5;
temp = (uint16_t)((float)(results[5]+resultsInv[5])*4.888);
txMsg.Data[1] = (temp>>8)&0xff;
txMsg.Data[2] = (temp)&0xff;
while (StdCan_Put(&txMsg) != StdCan_Ret_OK) {}
flowerChannelToSend = 0;
measureState=0;
break;
#endif
default:
measureState=0;
break;
}
if (measureState != 0) {
Timer_SetTimeout(sns_flower_MEASUREMENT_TIMER, sns_flower_CAN_MSG_PAUS_MS , TimerTypeOneShot, 0);
}
break;
}
}
}