void Sync_Main(void) {
static uint16 frac = FracN_DEFAULT;
static int16 prev_error = 0;
static uint8 pll_resetting = 0;
static uint16 pll_testing = PLL_VERIFY_TIME;
static uint16 pll_diff_acc = 0, pll_error_count = 0;
int16 cur_error, diff, p_term, d_term;
uint16 pos;
pos = CY_GET_REG8(SyncSOF_FRAME_POS_LO__STATUS_REG);
if (pos & 0x01) {
pos += (uint16)CY_GET_REG8(SyncSOF_FRAME_POS_HI__STATUS_REG) << 8;
cur_error = pos - SYNC_SOF_CENTER;
diff = cur_error - prev_error;
p_term = cur_error * SYNC_P_GAIN;
d_term = diff * SYNC_D_GAIN;
prev_error = cur_error;
frac += p_term + d_term;
if (frac > SYNC_FRAC_MAX) frac = SYNC_FRAC_MAX;
if (frac < SYNC_FRAC_MIN) frac = SYNC_FRAC_MIN;
if (pll_testing) {
if (pll_resetting) {
frac = 0;
// Wait for PLL reset
if (!(--pll_resetting)) {
Control_Write(Control_Read() & ~CONTROL_LED);
frac = FracN_DEFAULT;
FRAC_CLK_SetDividerValue(13);
}
} else {
pll_testing--;
if (diff < 0) pll_diff_acc -= diff;
else pll_diff_acc += diff;
pll_diff_acc /= 2;
if (pll_diff_acc < 2) {
if (pll_error_count) pll_error_count--;
} else if (pll_diff_acc > 7) {
pll_error_count++;
}
if (pll_error_count > 50) {
pll_diff_acc = 0;
pll_error_count = 0;
pll_resetting = PLL_RESET_TIME;
pll_testing = PLL_VERIFY_TIME;
Control_Write(Control_Read() | CONTROL_LED);
FRAC_CLK_SetDividerValue(14);
}
}
}
FracN_Set(frac);
}
}
// Example usage:
// Morse_Main("Repeating Message ");for(;;){sleep(240);Morse_Main(0);}
void Morse_Main(char* msg) {
static uint8 pos, codes, len, state, timer, *message;
uint8 i;
if (msg) {
state = pos = 0;
message = msg;
}
else switch (state) {
case 0:
if (!message[pos]) pos = 0;
i = message[pos++];
if (i >= 0x61 && i <= 0x7A) i -= 32;
if (i < 0x22 || i > 0x5A) codes = 7;
else codes = MCODES[i-0x22];
len = codes & 0x07;
if (len==0) len = 6;
if (codes==7) {
timer = MORSE_WORD - MORSE_CHAR - 2;
len = 0;
state = 3;
break;
}
case 1:
if (codes & 0x80) timer = MORSE_DASH;
else timer = MORSE_DOT;
codes <<= 1;
len--;
Control_Write(Control_Read() & ~CONTROL_LED);
state = 2;
case 2:
if (!timer) {
state = 3;
if (!len) timer = MORSE_CHAR - 1;
else timer = MORSE_DOT - 1;
Control_Write(Control_Read() | CONTROL_LED);
}
else {
timer--;
break;
}
case 3:
if (!timer) {
if (!len) state = 0;
else state = 1;
}
else timer--;
}
}
void TIM2_IRQHandler() {
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
int enabled = ((GPIOF->ODR & DRVF_EN) == 0);
int reading = Control_Read();
if((Control_GoNext == 0 && reading != Control_Destination0) || (Control_GoNext == 1 && (reading == Control_Destination0 || reading == 0))){
if(!enabled) {
// destination not reached, but outputs are not enabled.
// to enable outputs, set to low
GPIOF->BSRR = DRVF_EN << 16;
TIM2->ARR = Control_ConfigMaxInterval;
elapsed = 0;
} else if(elapsed >= 0) {
elapsed += TIM2->ARR;
}
if(GPIOA->ODR & DRVA_STEP) {
// high -> low
GPIOA->BSRR = DRVA_STEP << 16;
} else {
// low -> high
GPIOA->BSRR = DRVA_STEP;
int speed = TIM2->ARR;
if(elapsed >= 0) {
if(speed > Control_ConfigMinInterval) {
// decrease timer period = increase motor speed
speed = (Control_ConfigMaxInterval * Control_ConfigMultiplier + -Control_ConfigAcceleration*elapsed) / Control_ConfigMultiplier;
TIM2->ARR = speed;
} else {
elapsed = -1;
}
}
}
} else {
// position reached. check if via
if(Control_Destination1 != 0) {
// it's a via. set to new destination
Control_Destination0 = Control_Destination1;
Control_Destination1 = 0;
} else {
// final position reached. to disable outputs, set to high
GPIOF->BSRR = DRVF_EN;
TIM_Cmd(TIM2, DISABLE);
}
}
}
void T1_Main(void) {
static uint8 state = 0, timer, band, send, bits, band_request, band_request_timer;
static uint16 tune_timer;
if (tune_timer) {
tune_timer--;
if (!tune_timer) {
Control_Write(Control_Read() & ~CONTROL_ATU_1);
}
}
if (band_request_timer) band_request_timer--;
else if (T1_Band_Number != band) {
band_request = 1;
band_request_timer = 200;
}
switch(state) {
case 0: // idle
if (Status_Read() & STATUS_ATU_0) timer++;
else {
if (timer >= 85 && timer <= 115) {
state = 1;
timer = 20;
send = band = T1_Band_Number;
bits = 4;
Control_Write(Control_Read() & ~CONTROL_ATU_0 | CONTROL_ATU_0_OE);
}
else {
timer = 0;
if (band_request || T1_Tune_Request) {
Control_Write(Control_Read() | CONTROL_ATU_1);
if (T1_Tune_Request) tune_timer = 1000;
else tune_timer = 10;
band_request = T1_Tune_Request = 0;
}
}
}
break;
case 1: // data low
timer--;
if (!timer) {
if (bits) {
Control_Write(Control_Read() | CONTROL_ATU_0);
if (send & 0x08) timer = 8;
else timer = 3;
send <<= 1;
bits--;
state = 2;
} else {
Control_Write(Control_Read() & ~(CONTROL_ATU_0 | CONTROL_ATU_0_OE));
state = 0;
}
}
break;
case 2: // data high
timer--;
if (!timer) {
if (bits) timer = 3;
else timer = 10;
Control_Write(Control_Read() & ~CONTROL_ATU_0);
state = 1;
}
break;
}
}
void Control_RunNext() {
Control_GoNext = 1;
Control_Destination0 = Control_Read();
TIM_Cmd(TIM2, ENABLE);
}
void Control_HandleMessage(const CanRxMsg *msg) {
int cmd = msg->StdId & Comm_CMD_MASK;
CanTxMsg ans;
ans.IDE = CAN_Id_Standard;
ans.RTR = CAN_RTR_Data;
switch(cmd) {
case Comm_Control_Position_GET_REQ: {
ans.StdId = Comm_Control_Position_GET_ANS;
ans.DLC = 1;
ans.Data[0] = Control_Read();
} break;
case Comm_Control_Position_SET_REQ: {
ans.StdId = Comm_Control_Position_SET_ANS;
ans.DLC = 1;
if(msg->DLC == 1 || msg->DLC == 2) {
int dest0 = msg->Data[0] & 0b111111;
int dest1 = 0;
if(msg->DLC == 2) {
dest1 = msg->Data[1] & 0b111111;
}
Control_RunTo(dest0, dest1);
// report success
ans.Data[0] = 1;
} else {
// report error
ans.Data[0] = 0;
}
} break;
case Comm_Control_Config_GET_REQ: {
ans.StdId = Comm_Control_Config_GET_ANS;
ans.DLC = 8;
ans.Data[0] = Control_ConfigMaxInterval & 0xFF;
ans.Data[1] = (Control_ConfigMaxInterval >> 8) & 0xFF;
ans.Data[2] = Control_ConfigMinInterval & 0xFF;
ans.Data[3] = (Control_ConfigMinInterval >> 8) & 0xFF;
ans.Data[4] = Control_ConfigMultiplier & 0xFF;
ans.Data[5] = (Control_ConfigMultiplier >> 8) & 0xFF;
ans.Data[6] = Control_ConfigAcceleration & 0xFF;
ans.Data[7] = (Control_ConfigAcceleration >> 8) & 0xFF;
} break;
case Comm_Control_Config_SET_REQ: {
Control_Stop();
Control_ConfigMaxInterval = msg->Data[0] + (msg->Data[1] << 8);
Control_ConfigMinInterval = msg->Data[2] + (msg->Data[3] << 8);
Control_ConfigMultiplier = msg->Data[4] + (msg->Data[5] << 8);
Control_ConfigAcceleration = msg->Data[6] + (msg->Data[7] << 8);
ans.StdId = Comm_Control_Config_SET_ANS;
ans.DLC = 1;
ans.Data[0] = 1;
} break;
case Comm_Control_Command_REQ: {
ans.StdId = Comm_Control_Command_ANS;
ans.DLC = 0;
switch(msg->Data[0]) {
case ControlCommand_Step:
ans.DLC = 2;
ans.Data[0] = ControlCommand_Step;
ans.Data[1] = Control_Read();
Control_RunNext();
break;
case ControlCommand_Stop:
Control_Stop();
ans.DLC = 2;
ans.Data[0] = ControlCommand_Stop;
ans.Data[1] = Control_Read();
break;
case ControlCommand_IsRunning:
ans.DLC = 2;
ans.Data[0] = ControlCommand_IsRunning;
ans.Data[1] = Control_IsRunning();
}
} break;
default: return;
}
Comm_Send(&ans);
}
