void Modbus_Handler(void)
{
uint16_t command = 0;
if(*(usRegHoldingBuf + MODBUS_COMMAND) != 0)
{
command = *(usRegHoldingBuf + MODBUS_COMMAND);
*(usRegHoldingBuf + MODBUS_COMMAND) = 0;
switch(command)
{
case 1:
SaveEEPROM();
break;//save all
case 2:
LoadModbus();
break;//load
case 3:
LoadModbusDefault();
break;//default
case 4:
SaveModbus4();
break;//save 1
case 5:
SaveModbus5();
break;//save 2
case 6:
SaveModbus6();
break;//save 3
case 7:
EraseEEPROM();
break;
case 8:
CleanFlash();
break;
case 9:
// g_uartMode = Command;
break;
case 10:
// g_modbusReg4[modbusDefaultSet] = 0;//reboot
break;
default:
break;
}
}
}
void main()
{
InitHardware();
Encoder(true);
ResetServo();
delay_ms(200);
bEnable = !STEP_ENABLE;
// run these asynch tasks
for (;;){
if (!STEP_ENABLE){
HandleRXData();
if (bDirty && !bEnable){
bDirty = false;
SaveEEPROM();
}
}
}
}
void main()
{
InitHardware();
Encoder(true);
ResetServo();
delay_ms(200);
bEnable = !STEP_ENABLE;
// run these asynch tasks
for (;;){
if (bServo){
SYNC = true;
UpdatePosition();
UpdateTrajectory();
CalculateExtCount();
UpdatePID();
UpdatePWM();
HandleErrorTask();
bServo = false;
} else {
//HandleRXData();
if (SYNC){
if (bDirty && !bEnable){
bDirty = false;
SaveEEPROM();
}
SYNC = false;
}
}
}
}
void UpdateTrajectory()
{
STATUS.index = INDEX_PIN;
if (STATUS.index && STATUS.halt_index || STATUS.error){
velact = 0;
return;
}
switch (mode){
case e_velo_mode:
if (!(!STATUS.halt_index && STATUS.saturated)){
if ((long)Fix2LongInt(velact.ul) < vars.velcom){ // is the velocity lower than target
velact.sl += vars.accel;
if ((long)Fix2LongInt(velact.ul) > vars.velcom)
velact = LongInt2Fix(vars.velcom,0);
} else if ((long)Fix2LongInt(velact.ul) > vars.velcom){
velact.sl -= vars.accel;
if ((long)Fix2LongInt(velact.ul) < vars.velcom)
velact = LongInt2Fix(vars.velcom,0);
}
cmd_pos.sl += Fix2LongInt(velact.ul);
}
break;
case e_pos_mode:
if (!(!STATUS.halt_index && STATUS.saturated)){
if (!FLAGS.phase){ // process the first half of the profile
if ((long)Fix2LongInt(velact.ul) < vars.vlim) // we need to accelerate to start move
velact.sl += vars.accel; // keep adding accel value to actual velocity
else
flatcount++; // keep track of visits through this function
// after accel has peaked
phase1dist.sl -= Fix2LongInt(velact.ul);
if (FLAGS.neg_move)
cmd_pos.sl -= Fix2LongInt(velact.ul); // keep moving position
else
cmd_pos.sl += Fix2LongInt(velact.ul); // keep moving position forward
if (phase1dist.sl < 0)
FLAGS.phase = true; // if phase distance is neg, then we are finished with first half of profile
} else {
// this is the second half of the profile
if (flatcount > 0){
flatcount--;
} else if ((long)Fix2LongInt(velact.ul)>0 ){
velact.sl -= vars.accel;
} else {
cmd_pos = LongInt2Fix(fposition,0);
STATUS.move_in_prog = false;
mode = e_hold_mode;
}
if (FLAGS.neg_move) // keep moving position
cmd_pos.sl -= Fix2LongInt(velact.ul);
else
cmd_pos.sl += Fix2LongInt(velact.ul);
}
}
break;
case e_save_eeprom_mode:
mode = e_hold_mode;
SaveEEPROM();
break;
case e_pos_go_mode:
StartGoMove();
break;
case e_servo_reset_mode:
ResetServo();
break;
case e_servo_lock_mode:
LockServo();
break;
case e_servo_unlock_mode:
STATUS.pid_lock = false;
mode = e_hold_mode;
break;
}
}
