169 lines
3.7 KiB
C++
169 lines
3.7 KiB
C++
void encodersInit()
|
|
{
|
|
encoders[0].reset(SLOT1);
|
|
encoders[1].reset(SLOT2);
|
|
encoders[2].reset(SLOT3);
|
|
encoders[3].reset(SLOT4);
|
|
attachInterrupt(encoders[0].getIntNum(), isr_process_encoder1, RISING);
|
|
attachInterrupt(encoders[1].getIntNum(), isr_process_encoder2, RISING);
|
|
attachInterrupt(encoders[2].getIntNum(), isr_process_encoder3, RISING);
|
|
attachInterrupt(encoders[3].getIntNum(), isr_process_encoder4, RISING);
|
|
|
|
measurement_speed_time = lasttime_speed = lasttime_angle = millis();
|
|
encoders[0].setPulsePos(0);
|
|
encoders[1].setPulsePos(0);
|
|
encoders[2].setPulsePos(0);
|
|
encoders[3].setPulsePos(0);
|
|
PID_speed_left.Setpoint = 0;
|
|
PID_speed_right.Setpoint = 0;
|
|
}
|
|
void encodersUpdate()
|
|
{
|
|
for(int i=0;i<4;i++)
|
|
{
|
|
encoders[i].loop();
|
|
}
|
|
}
|
|
void onEncoderMovingFinish(int slot,int extId)
|
|
{
|
|
writeHead();
|
|
writeSerial(extId);
|
|
sendByte(slot);
|
|
writeEnd();
|
|
}
|
|
void PWM_Calcu()
|
|
{
|
|
double speed1;
|
|
double speed2;
|
|
if((millis() - lasttime_speed) > 20)
|
|
{
|
|
speed1 = encoders[0].getCurrentSpeed();
|
|
speed2 = encoders[1].getCurrentSpeed();
|
|
|
|
#ifdef DEBUG_INFO
|
|
Serial.print("S1: ");
|
|
Serial.print(speed1);
|
|
Serial.print(" S2: ");
|
|
Serial.print(speed2);
|
|
Serial.print("left: ");
|
|
Serial.print(PID_speed_left.Setpoint);
|
|
Serial.print(" right: ");
|
|
Serial.println(PID_speed_right.Setpoint);
|
|
#endif
|
|
|
|
if(abs(abs(PID_speed_left.Setpoint) - abs(PID_speed_right.Setpoint)) >= 0)
|
|
{
|
|
encoders[0].setMotorPwm(PID_speed_left.Setpoint);
|
|
encoders[1].setMotorPwm(PID_speed_right.Setpoint);
|
|
return;
|
|
}
|
|
|
|
if((abs(PID_speed_left.Setpoint) == 0) && (abs(PID_speed_right.Setpoint) == 0))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if(abs(speed1) - abs(speed2) >= 0)
|
|
{
|
|
if(PID_speed_left.Setpoint > 0)
|
|
{
|
|
encoders[0].setMotorPwm(PID_speed_left.Setpoint - (abs(speed1) - abs(speed2)));
|
|
encoders[1].setMotorPwm(PID_speed_right.Setpoint);
|
|
}
|
|
else
|
|
{
|
|
encoders[0].setMotorPwm(PID_speed_left.Setpoint + (abs(speed1) - abs(speed2)));
|
|
encoders[1].setMotorPwm(PID_speed_right.Setpoint);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(PID_speed_right.Setpoint > 0)
|
|
{
|
|
encoders[0].setMotorPwm(PID_speed_left.Setpoint);
|
|
encoders[1].setMotorPwm(PID_speed_right.Setpoint - (abs(speed2) - abs(speed1)));
|
|
}
|
|
else
|
|
{
|
|
encoders[0].setMotorPwm(PID_speed_left.Setpoint);
|
|
encoders[1].setMotorPwm(PID_speed_right.Setpoint + (abs(speed2) - abs(speed1)));
|
|
}
|
|
}
|
|
lasttime_speed = millis();
|
|
}
|
|
}
|
|
|
|
void Forward(void)
|
|
{
|
|
// Encoder_1.setMotorPwm(-moveSpeed);
|
|
// Encoder_2.setMotorPwm(moveSpeed);
|
|
PID_speed_left.Setpoint = -moveSpeed;
|
|
PID_speed_right.Setpoint = moveSpeed;
|
|
move_status = MOVE_FORWARD;
|
|
PWM_Calcu();
|
|
}
|
|
|
|
void Backward(void)
|
|
{
|
|
// Encoder_1.setMotorPwm(moveSpeed);
|
|
// Encoder_2.setMotorPwm(-moveSpeed);
|
|
PID_speed_left.Setpoint = moveSpeed;
|
|
PID_speed_right.Setpoint = -moveSpeed;
|
|
move_status = MOVE_BACKWARD;
|
|
PWM_Calcu();
|
|
}
|
|
|
|
void Stop(void)
|
|
{
|
|
encoders[0].setMotorPwm(0);
|
|
encoders[1].setMotorPwm(0);
|
|
move_status = MOVE_STOP;
|
|
}
|
|
void isr_process_encoder1(void)
|
|
{
|
|
if(digitalRead(encoders[0].getPortB()) == 0)
|
|
{
|
|
encoders[0].pulsePosMinus();
|
|
}
|
|
else
|
|
{
|
|
encoders[0].pulsePosPlus();
|
|
}
|
|
}
|
|
|
|
void isr_process_encoder2(void)
|
|
{
|
|
if(digitalRead(encoders[1].getPortB()) == 0)
|
|
{
|
|
encoders[1].pulsePosMinus();
|
|
}
|
|
else
|
|
{
|
|
encoders[1].pulsePosPlus();
|
|
}
|
|
}
|
|
|
|
void isr_process_encoder3(void)
|
|
{
|
|
if(digitalRead(encoders[2].getPortB()) == 0)
|
|
{
|
|
encoders[2].pulsePosMinus();
|
|
}
|
|
else
|
|
{
|
|
encoders[2].pulsePosPlus();
|
|
}
|
|
}
|
|
|
|
void isr_process_encoder4(void)
|
|
{
|
|
if(digitalRead(encoders[3].getPortB()) == 0)
|
|
{
|
|
encoders[3].pulsePosMinus();
|
|
}
|
|
else
|
|
{
|
|
encoders[3].pulsePosPlus();
|
|
}
|
|
}
|