void __ISR(_TIMER_4_VECTOR, ipl2) _InterruptHandler_TMR4(void)
{
// if(current_block->direction_bits[Z_AXIS])
// PORTSetBits(zAxis.directionPin.port, zAxis.directionPin.pin);
// else
// PORTClearBits(zAxis.directionPin.port, zAxis.directionPin.pin);
if(steps_Z)
{
if(!(mPORTAReadBits(zAxis.enablePin.pin)))
steps_Z--;
}
if (current_block != Null)
{
if (current_block->steps_z)
{
if(!(mPORTAReadBits(zAxis.enablePin.pin)))
current_block->steps_z--;
}
else
{
//CloseOC2();
BSP_AxisDisable(Z_AXIS);
BSP_Timer4Stop();
}
}
else
{
BSP_AxisDisable(Z_AXIS);
}
// clear the interrupt flag
mT4ClearIntFlag();
}
static PT_THREAD(protothread_CSense(struct pt *pt)) {
PT_BEGIN(pt);
while (1) {
if (config1 & CUR_SENSE_EN) {
if(Status2 & M7_CUR){
if(M7_LastDir != M7_Dir)//if direction has changed, lower CS flag, which enables PWM writing
Status2 &= !M7_CUR;
}
if(Status2 & M6_CUR){
if(M6_LastDir != M6_Dir)
Status2 &= !M6_CUR;
}
if(Status2 & M6_CUR){
if(M6_LastDir != M6_Dir)
Status2 &= !M6_CUR;
}
if(Status2 & M5_CUR){
if(M5_LastDir != M5_Dir)
Status2 &= !M5_CUR;
}
if(Status2 & M4_CUR){
if(M4_LastDir != M4_Dir)
Status2 &= !M4_CUR;
}
if(Status2 & M3_CUR){
if(M3_LastDir != M3_Dir)
Status2 &= !M3_CUR;
}
if (mPORTBReadBits(BIT_3)) {//CS triggered!
M7_PWM = 0;
SetDCOC1PWM(M7_PWM);//Halt motor
Status2 |= M7_CUR;//set CS flag
M7_LastDir = M7_Dir;//save direction
}
if (mPORTBReadBits(BIT_1)) {
M6_PWM = 0;
SetDCOC2PWM(M6_PWM);
Status2 |= M6_CUR;
M6_LastDir = M6_Dir;
}
if (mPORTAReadBits(BIT_3)) {
M5_PWM = 0;
SetDCOC3PWM(M5_PWM);
Status2 |= M5_CUR;
M5_LastDir = M5_Dir;
}
if (mPORTBReadBits(BIT_4)) {
M4_PWM = 0;
SetDCOC5PWM(M4_PWM);
Status2 |= M4_CUR;
M4_LastDir = M4_Dir;
}
if (mPORTAReadBits(BIT_4)) {
M3_PWM = 0;
SetDCOC4PWM(M3_PWM);
Status2 |= M3_CUR;
M3_LastDir = M3_Dir;
}
}
PT_YIELD_TIME_msec(10);
}
PT_END(pt);
}