File content as of revision 6:ff3c66f7372b:

/* 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"

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