File content as of revision 8:00558287a4ef:

/* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
 *
 * File: motors.cpp
 *
 * (C) Dept. Electronics & Computer Science, University of York
 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
 *
 * PsiSwarm Library Version: 0.3
 *
 * October 2015
 *
 */

#include "psiswarm.h"

Timeout time_based_action_timeout;

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

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

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

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

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

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

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

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

void 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();
}

void time_based_turn(float speed, int microseconds)
{
}

void time_based_turn_degrees(float speed, float degrees)
{
    if(speed < 0 || speed > 1 || degrees == 0) {
    debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
    } else {
        char invert = 0;
        if(degrees < 0) {degrees=-degrees; invert = 1;}
        
        //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;
        }
        
        //pc.printf("Speed: %f   Turn time: %f\n",speed,turn_time);
        if(invert) speed=-speed;
        turn(speed);
        time_based_action_timeout.attach(&IF_end_time_based_action,turn_time);
}
}


void IF_end_time_based_action()
{
        brake();
}

void 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_speed(motor_left_speed));

        } else {
            motor_left_r.write(0);
            motor_left_f.write(IF_calibrated_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_speed(motor_right_speed));
        } else {
            motor_right_r.write(0);
            motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
        }
    }

}


float 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 IF_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();
}