motors.cpp

/* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
 *
 * Copyright 2016 University of York
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and limitations under the License.
 *
 * File: motors.cpp
 *
 * (C) Dept. Electronics & Computer Science, University of York
 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
 *
 * PsiSwarm Library Version: 0.8
 *
 * October 2016
 *
 *
 */

#include "psiswarm.h"

Timeout time_based_action_timeout;
char brake_when_done = 0;

void Motors::set_left_motor_speed(float speed)
{
    motor_left_speed = speed;
    motor_left_brake = 0;
    IF_update_motors();
}

void Motors::set_right_motor_speed(float speed)
{
    motor_right_speed = speed;
    motor_right_brake = 0;
    IF_update_motors();
}

void Motors::brake_left_motor()
{
    motor_left_speed = 0;
    motor_left_brake = 1;
    IF_update_motors();
}

void Motors::brake_right_motor()
{
    motor_right_speed = 0;
    motor_right_brake = 1;
    IF_update_motors();
}

void Motors::brake()
{
    motor_left_speed = 0;
    motor_right_speed = 0;
    motor_left_brake = 1;
    motor_right_brake = 1;
    IF_update_motors();
}

void Motors::stop()
{
    motor_left_speed = 0;
    motor_right_speed = 0;
    motor_left_brake = 0;
    motor_right_brake = 0;
    IF_update_motors();
}

void Motors::forward(float speed)
{
    motor_left_speed = speed;
    motor_right_speed = speed;
    motor_left_brake = 0;
    motor_right_brake = 0;
    IF_update_motors();
}

void Motors::backward(float speed)
{
    motor_left_speed = -speed;
    motor_right_speed = -speed;
    motor_left_brake = 0;
    motor_right_brake = 0;
    IF_update_motors();
}

void Motors::turn(float speed)
{
    //A positive turn is clockwise
    motor_left_speed = speed;
    motor_right_speed = -speed;
    motor_left_brake = 0;
    motor_right_brake = 0;
    IF_update_motors();
}

//Forward for a set period of time
void Motors::time_based_forward(float speed, int microseconds, char brake)
{
    //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
    IF_check_time_for_existing_time_based_action();

    //Start moving
    forward(speed);
    brake_when_done = brake;
    time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,microseconds);
}

//Turn for a set period of time
void Motors::time_based_turn(float speed, int microseconds, char brake)
{
    //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
    IF_check_time_for_existing_time_based_action();

    //Start turning
    turn(speed);
    brake_when_done = brake;
    time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,microseconds);
}

//Returns the limit of degrees available to turn in given time
float Motors::get_maximum_turn_angle(int microseconds)
{
    //We can turn at about 270 degrees per second at full speed
    return (microseconds * 0.00027);
}

//Return the time in microseconds that performing the turn will take
int Motors::get_time_based_turn_time(float speed, float degrees)
{
    //Check sign of degrees
    if(degrees < 0) degrees =- degrees;

    //Main calculation for turn time
    float turn_time = degrees / ((290 * speed));

    //Add a hard offset of 4ms to account for start\stop time
    if(degrees > 4) {
        turn_time += 0.004;
    } else turn_time += 0.002;

    // Add offset for slow speed
    if(speed<0.31) {
        float mul_fact = 0.31 - speed;
        if(mul_fact < 0) mul_fact = 0;
        mul_fact /= 2;
        mul_fact += 1;
        turn_time *= mul_fact;
    }

    // Add offset for short turns
    if(degrees < 360) {
        float short_offset_multiplier = 1.0 + (0.9 / degrees);
        turn_time *= short_offset_multiplier;
    }

    // Convert to uS
    turn_time *= 1000000;

    return (int) turn_time;
}

//Turn the robot a set number of degrees [using time estimation to end turn]
int Motors::time_based_turn_degrees(float speed, float degrees, char brake)
{
    if(speed < 0 || speed > 1 || degrees == 0) {
        psi.debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
        return 0;
    } else {
        //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
        IF_check_time_for_existing_time_based_action();

        //Calculate turn time using get_time_based_turn_time
        int turn_time = get_time_based_turn_time(speed,degrees);

        //Set correct turn direction (-degrees is a counter-clockwise turn)
        if(degrees < 0) {
            degrees=-degrees;
            speed=-speed;
        }

        //Start turning
        turn(speed);

        brake_when_done = brake;
        time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,turn_time);
        return turn_time;
    }
}

//Check if a current time based action is running - if it is, throw a warning and cancel its timeout
void Motors::IF_check_time_for_existing_time_based_action()
{
    if(time_based_motor_action == 1) {
        time_based_action_timeout.detach();
        psi.debug("WARNING: New time-based action called before previous action finished!\n");
    } else time_based_motor_action = 1;
}

void Motors::IF_end_time_based_action()
{
    if(brake_when_done == 1)brake();
    else stop();
    time_based_motor_action = 0;
}

void Motors::IF_update_motors()
{
    if(motor_left_speed > 1.0) {
        motor_left_speed = 1.0;
        //Throw exception...
    }
    if(motor_right_speed > 1.0) {
        motor_right_speed = 1.0;
        //Throw exception...
    }
    if(motor_left_speed < -1.0) {
        motor_left_speed = -1.0;
        //Throw exception...
    }
    if(motor_right_speed < -1.0) {
        motor_right_speed = -1.0;
        //Throw exception...
    }
    if(motor_left_brake) {
        motor_left_f.write(1);
        motor_left_r.write(1);
        if(motor_left_speed!=0) {
            motor_left_speed = 0;
            //Throw exception...
        }
    } else {
        if(motor_left_speed >= 0) {
            motor_left_f.write(0);
            motor_left_r.write(IF_calibrated_left_speed(motor_left_speed));

        } else {
            motor_left_r.write(0);
            motor_left_f.write(IF_calibrated_left_speed(-motor_left_speed));
        }
    }
    if(motor_right_brake) {
        motor_right_f.write(1);
        motor_right_r.write(1);
        if(motor_right_speed!=0) {
            motor_right_speed = 0;
            //Throw exception...
        }
    } else {
        if(motor_right_speed >= 0) {
            motor_right_f.write(0);
            motor_right_r.write(IF_calibrated_right_speed(motor_right_speed));
        } else {
            motor_right_r.write(0);
            motor_right_f.write(IF_calibrated_right_speed(-motor_right_speed));
        }
    }

}


float Motors::IF_calibrated_left_speed(float speed)
{
    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
    float adjusted  = IF_calibrated_speed(speed);
    if(use_motor_calibration) {
        adjusted *= left_motor_calibration_value;
    }
    return adjusted;
}

float Motors::IF_calibrated_right_speed(float speed)
{
    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
    float adjusted  = IF_calibrated_speed(speed);
    if(use_motor_calibration) {
        adjusted *= right_motor_calibration_value;
    }
    return adjusted;
}

float Motors::IF_calibrated_speed(float speed)
{
    if(speed == 0) return 0;
    //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
    float adjusted = speed;
    if(OFFSET_MOTORS) {
        adjusted*=0.8f;
        adjusted+=0.2;
    }
    return adjusted;
}

void Motors::init_motors()
{
    // Motor PWM outputs work optimally at 25kHz frequency
    motor_left_f.period_us(40);
    motor_right_f.period_us(40);
    motor_left_r.period_us(40);
    motor_right_r.period_us(40);
    motor_left_speed = 0;
    motor_right_speed = 0;
    motor_left_brake = 0;
    motor_right_brake = 0;
    IF_update_motors();
}