motor.cpp

#include "motor.h"

//CONSTRUCTORS
Motor :: Motor (MotorHardware_t& stepper): stepper(stepper)
{
    set_physical(300,300, 1);
    set_accel_mm_per_sec_per_sec(0.5); //  <----- this is not yet configured by user
}


//SETTERS
void Motor :: set_physical (int steps_per_revolution, float mm_per_revolution, bool forward_clockwise)
{
//Useful so the user can change motor caracteristics
    set_steps_per_revolution(steps_per_revolution);
    set_mm_per_revolution(mm_per_revolution);
    set_clockwise_equals_forward(forward_clockwise);
}

void Motor :: set_steps_per_revolution(float steps_per_revolution)
{
    _steps_per_revolution = steps_per_revolution;
}

void Motor :: set_mm_per_revolution (float mm_per_revolution)
{
    _mm_per_revolution = mm_per_revolution;
}

void Motor :: set_accel_mm_per_sec_per_sec (float acceleration_mm_per_sec_per_sec)
{
    if (acceleration_mm_per_sec_per_sec == 0)
        acceleration_mm_per_sec_per_sec = 1000;
    _acceleration_mm_per_sec_per_sec = acceleration_mm_per_sec_per_sec;
#if !CORE_MOCK
    stepper.setAcceleration(mm_to_step(acceleration_mm_per_sec_per_sec));
#endif
}

void Motor :: set_clockwise_equals_forward (bool forward_clockwise)
{
    _clockwise_equals_forward = forward_clockwise;
}

void Motor :: set_speed_mm_per_sec (float mm_per_sec)
{
#if !CORE_MOCK
    stepper.setMaxSpeed(mm_to_step(mm_per_sec));
#endif
}


//GETTERS
float Motor :: get_steps_per_revolution()
{
    return _steps_per_revolution;
}

float Motor :: get_mm_per_revolution () const
{
    return _mm_per_revolution;
}

bool Motor :: get_clockwise_equals_forward ()
{
    return _clockwise_equals_forward;
}

float Motor :: get_accel_mm_per_sec_per_sec () const
{
    return _acceleration_mm_per_sec_per_sec;
}


//CONVERSIONS
float Motor :: mm_to_step (float mm) const
/***
-Argument : Distance in mm.
-Return   : The number of step associated.
-Action   : /
***/
{
    return mm * _steps_per_revolution / _mm_per_revolution;
}

float Motor :: step_to_mm (int step) const
/***
-Argument : Number of step.
-Return   : Distance in mm associated.
-Action   : /
***/
{
    return step * _mm_per_revolution / _steps_per_revolution;
}

//MOVEMENTS
void Motor :: move_to_step (int step)
/***
-Argument : Number of step.
-Return   : /
-Action   : Move the number of step required.
***/
{
#if !CORE_MOCK
    cli();
    set_accel_mm_per_sec_per_sec(_acceleration_mm_per_sec_per_sec);
    if (_mm_per_revolution > 0 && _steps_per_revolution > 0)
        stepper.moveTo(_clockwise_equals_forward? step: -step);
    sei();
#endif
    Serial.printf("#     move to: %d step    %g mm\n", step, step_to_mm(step));
}

void Motor :: move_to_mm (float mm)
/***
-Argument : distace in mm
-Return   : /
-Action   : Move to the distance required.
***/
{
    Serial.printf("# moving to %g mm / %g steps\n",
        mm,
        mm_to_step(mm));
    move_to_step(mm_to_step(mm));
}

void Motor :: stop ()
/***
-Argument : /
-Return   : /
-Action   : Stop the motor.
***/
{
#if !CORE_MOCK
    cli();
    stepper.stop();
    sei();
#endif
}

void Motor :: stay_here ()
/***
-Argument : /
-Return   : /
-Action   : Stop the movement but the motor is still running (it helps to maintain the position more precisely if there is a resistance)
***/
{
#if !CORE_MOCK
    cli();
    stepper.setAcceleration(1e20);
        stepper.moveTo(stepper.currentPosition()); // change target to here
    set_accel_mm_per_sec_per_sec(_acceleration_mm_per_sec_per_sec);
    sei();
#endif
}

bool Motor :: motor_is_running ()  //return true if the motor is currently running
/***
-Argument : /
-Return   : A boolean to indicates if the motor is running.
-Action   : /
***/
{
    return (stepper.speed() != 0);
}


//POSITION
int Motor :: where_step ()
/***
-Argument : /
-Return   : The current position in step.
-Action   : /
***/
{
#if CORE_MOCK
    return 0;
#else
    return stepper.currentPosition() * (_clockwise_equals_forward? 1: -1);
#endif
}

int Motor :: where_mm ()
/***
-Argument : /
-Return   : The current position in mm.
-Action   : /
***/
{
#if CORE_MOCK
    return 0;
#else
    return step_to_mm(stepper.currentPosition() * (_clockwise_equals_forward? 1: -1));
#endif
}

void Motor :: reset_position()
/***
-Argument : /
-Return   : /
-Action   : Set the curent position as the zero.
***/
{Motor
#if !CORE_MOCK
    stop();
    cli();
    stepper.setCurrentPosition(0);
    sei();
#endif
}

//CONFIGURATION
void Motor :: show_configuration ()
/***
-Argument : /
-Return   : /
-Action   : Print the current motor configuration.
***/
{
    Serial.printf("#     %g mm/rev\n"
                    "#     %d steps/rev\n"
                    "#     %g steps/mm\n"
                    "#     max speed: %g steps/s   %g mm/s\n"
                    "#     accel: %g steps/s/s   %g mm/s/s\n",
                    _mm_per_revolution,
                    _steps_per_revolution,
                    _steps_per_revolution / _mm_per_revolution,
                    stepper.maxSpeed(),
                    step_to_mm(stepper.maxSpeed()),
                    stepper.acceleration(),
                    step_to_mm(stepper.acceleration())
                );
}