void setRightPWM(int value){
MoteurD.setSpeed(abs(value));
if(value>0)
MoteurD.run(FORWARD);
else
MoteurD.run(BACKWARD);
}
void setLeftPWM(int value){
MoteurG.setSpeed(abs(value));
if(value<0)
MoteurG.run(FORWARD);
else
MoteurG.run(BACKWARD);
}
void initPWM(){
MoteurG = AF_DCMotor(1, MOTOR12_64KHZ);
MoteurD = AF_DCMotor(2, MOTOR12_64KHZ);
MoteurG.setSpeed(0);
MoteurG.run(FORWARD);
MoteurD.setSpeed(0);
MoteurD.run(FORWARD);
}
MotorControl::MotorControl(int leftForward, int rightForward, int leftReverse, int rightReverse){
Serial.begin(9600);
Serial.print("MotorControl initiated");
_leftForward=leftForward;
_rightForward=rightForward;
_leftReverse=leftReverse;
_rightReverse=rightReverse;
motorLeft.run(RELEASE);
motorRight.run(RELEASE);
}
void motor_setup()
{
lift_motor.setSpeed( RAW_LIFT_DOWN_SPEED );
lat_stepper.setSpeed( RAW_LAT_SPEED );
lift_action = MOTOR_STOP;
lat_action = MOTOR_STOP;
}
void set_lift_action_auto( motor_direction dir )
{
/*if( interlocked_up() || interlocked_down() ) {
return;
}*/
lift_motor_manual = false;
//lift_action = dir;
lift_motor_reference = get_lift_cm();
switch( dir ) {
case MOTOR_UP:
if( interlocked_up() ) {
return;
}
lift_action = dir;
//lift_motor.run( FORWARD );
//lift_motor.setSpeed( RAW_LIFT_UP_SPEED );
break;
case MOTOR_DOWN:
if( interlocked_down() ) {
return;
}
lift_action = dir;
//lift_motor.run( BACKWARD );
//lift_motor.setSpeed( RAW_LIFT_DOWN_SPEED );
break;
case MOTOR_STOP:
default:
lift_action = dir;
lift_motor.run( RELEASE );
break;
}
}
void set_pwm_right(int pwm){
if (pwm > 0)
pwm += PWM_MIN;
else if (pwm < 0)
pwm -= PWM_MIN;
if(pwm > 255)
pwm = 255;
else if(pwm < -255)
pwm = -255;
if(pwm >= 0)
motor_right.run(FORWARD);
else
motor_right.run(BACKWARD);
motor_right.setSpeed(abs(pwm));
}
void set_pwm_left(int pwm){
pwm = -pwm;//les moteurs sont faces à face, pour avancer il faut qu'il tournent dans un sens différent
if (pwm > 0)
pwm += PWM_MIN;
else if (pwm < 0)
pwm -= PWM_MIN;
if(pwm > 255)
pwm = 255;
else if(pwm < -255)
pwm = -255;
if(pwm >= 0)
motor_left.run(FORWARD);
else
motor_left.run(BACKWARD);
motor_left.setSpeed(abs(pwm));
}
void pump(bool etat) {
static bool saved_etat = etat;
if (etat == PAUSE_PUMP) {
digitalWrite(PIN_VALVE, HIGH);
pump_motor.setSpeed(0);
} else {
if (etat == RESUME_PUMP) {
etat = saved_etat;
} else {
saved_etat = etat;
}
if (etat) {
digitalWrite(PIN_VALVE, LOW);
pump_motor.setSpeed(PWM_PUMP);
} else {
digitalWrite(PIN_VALVE, HIGH);
pump_motor.setSpeed(0);
}
}
}
void set_lift_action( motor_direction dir )
{
// override interlock on non-auto
/*if( interlocked_up() || interlocked_down() ) {
return;
}*/
lift_motor_manual = true;
lift_action = dir;
switch( dir ) {
case MOTOR_UP:
lift_motor.run( FORWARD );
lift_motor.setSpeed( RAW_LIFT_UP_SPEED );
break;
case MOTOR_DOWN:
lift_motor.run( BACKWARD );
lift_motor.setSpeed( RAW_LIFT_DOWN_SPEED );
break;
case MOTOR_STOP:
default:
lift_motor.run( RELEASE );
break;
}
}
void initAct() {
//Moteurs :
pump_motor.run(FORWARD);
cmdBrasServ(ANGLE_DEPOT, LONGUEUR_DEPOT);
servoRet.write(150);
pump(true);
stepperAsc.setAcceleration(AMAX_STEPPER);
stepperAsc.setMaxSpeed(VMAX_STEPPER);
stepperAsc.move(6000);
while (digitalRead(PIN_INT_HAUT_ASC) == 1) {
updateBras();
}
topStop();
pump(false);
cmdBrasServ(ANGLE_DEPOT, LONGUEUR_DEPOT);
cmdAsc(HAUTEUR_GARDE_DEPOT);
while (!next_step)
stepperAsc.run();
next_step = true;
servoRet.write(180);
}
void motor_loop()
{
// lateral motor run
if( lat_action == MOTOR_RIGHT && interlocked_right() ||
lat_action == MOTOR_LEFT && interlocked_left() )
{
Serial.println( "Lat interlocked" );
set_lat_action( MOTOR_STOP );
}
if( lat_action != MOTOR_STOP ) {
lat_stepper.runSpeed();
}
// lift motor run (if auto)
// time stuff:
unsigned long time = millis();
static long next_sample_time = 0;
if( next_sample_time == 0 ) {
next_sample_time = time;
}
// control loop
if( time > next_sample_time ) {
next_sample_time += LIFT_CONTROL_LOOP_MS;
if( !lift_motor_manual ) {
float error;
switch( lift_action ) {
case MOTOR_UP:
if( get_lift_cm() > LIFT_UPPER_LIMIT || interlocked_up() ) {
lift_action = MOTOR_STOP;
lift_motor.run( RELEASE );
break;
}
lift_motor.run( FORWARD );
lift_motor_reference += LIFT_CONTROL_CM_PER_LOOP;
error = lift_motor_reference - get_lift_cm();
if( error > 0 ) {
lift_motor.setSpeed( Kp * error );
} else {
lift_motor.setSpeed( 0 );
}
break;
case MOTOR_DOWN:
if( get_lift_cm() < LIFT_LOWER_LIMIT || interlocked_down() ) {
lift_action = MOTOR_STOP;
lift_motor.run( RELEASE );
break;
}
lift_motor.run( BACKWARD );
lift_motor_reference -= LIFT_CONTROL_CM_PER_LOOP;
error = lift_motor_reference - get_lift_cm();
if( error < 0 ) {
lift_motor.setSpeed( -Kp * error );
} else {
lift_motor.setSpeed( 0 );
}
break;
case MOTOR_STOP:
//lift_motor.run( RELEASE );
break;
}
}
}
}
void MotorControl::backward(){
motorLeft.setSpeed(_leftReverse);
motorRight.setSpeed(_rightReverse);
motorLeft.run(BACKWARD);
motorRight.run(BACKWARD);
}
void MotorControl::brake(){
motorLeft.run(RELEASE);
motorRight.run(RELEASE);
}
void MotorControl::turnRight(){
motorLeft.setSpeed(_leftForward);
motorRight.setSpeed(_rightReverse);
motorLeft.run(FORWARD);
motorRight.run(BACKWARD);
}
void MotorControl::forward(){
motorLeft.setSpeed(_leftForward);
motorRight.setSpeed(_rightForward);
motorLeft.run(FORWARD);
motorRight.run(FORWARD);
}
