File content as of revision 1:ffd9a51e7d35:

/*
** This software can be freely used, even comercially, as highlighted in the license.
** 
** Copyright 2014 GHI Electronics, LLC
** 
** 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.
**
**/
#include "mBot.h"

    PwmOut *mBot::leftMotor = NULL;
    PwmOut *mBot::rightMotor = NULL;
    
    DigitalOut *mBot::leftMotorDirectionCtrl_1 = NULL;
    DigitalOut *mBot::leftMotorDirectionCtrl_2 = NULL;
    
    DigitalOut *mBot::rightMotorDirectionCtrl_1 = NULL;
    DigitalOut *mBot::rightMotorDirectionCtrl_2 = NULL;

    bool mBot::leftMotorInverted = false;
    bool mBot::rightMotorInverted = false;
    
    AnalogIn *mBot::leftSensor = NULL;
    AnalogIn *mBot::rightSensor = NULL;
    DigitalOut *mBot::leftIRLED = NULL;
    DigitalOut *mBot::rightIRLED = NULL;


mBot::mBot()
{

    leftSensor = new AnalogIn(P0_14);
    rightSensor = new AnalogIn(P0_15);
    
    leftIRLED = new DigitalOut(P0_23,1);    // FIX: poweraving?
    rightIRLED = new DigitalOut(P0_17,1);
    
    leftMotor = new PwmOut(P0_9);
    leftMotor->period(MOTOR_BASE_PERIOD); // float 1/frequency (seconds)
    leftMotor->write(0.5);  // duty cycle   
    leftMotorDirectionCtrl_1 = new DigitalOut(P0_7, 0);
    leftMotorDirectionCtrl_2 = new DigitalOut(P0_11, 0);
    

    rightMotor = new PwmOut(P0_8);
    rightMotor->period(MOTOR_BASE_PERIOD);
    rightMotor->write(0.5);  // duty cycle
    rightMotorDirectionCtrl_1 = new DigitalOut(P0_12,0);
    rightMotorDirectionCtrl_2 = new DigitalOut(P0_13,0);

    
    leftMotorInverted = 0;
    rightMotorInverted = 0;

}


float mBot::GetReflectiveReading(ReflectiveSensors sensor)
{
    return sensor == Left ? leftSensor->read() : rightSensor->read();
}

void mBot::SetReflectiveSensorState(int state)
{
    leftIRLED->write(state);
    rightIRLED->write(state);
}

void mBot::SetMotorInversion(bool invertLeft, bool invertRight)
{
    leftMotorInverted = invertLeft;
    rightMotorInverted = invertRight;
}

void mBot::SetMotorSpeed(int leftSpeed, int rightSpeed)
{
    if (leftSpeed > 100 || leftSpeed < -100 || rightSpeed > 100 || rightSpeed < -100)
    {
        return;
    }
    
    if ((leftSpeed | rightSpeed) == 0) {
        leftMotorDirectionCtrl_1->write(0);
        leftMotorDirectionCtrl_2->write(0);
        rightMotorDirectionCtrl_1->write(0);
        rightMotorDirectionCtrl_2->write(0);
        return;
    };
        
        

    if (leftMotorInverted)
    {
        leftSpeed *= -1;
    }

    if (rightMotorInverted)
    {
        rightSpeed *= -1;
    }
    
    
    SetSpeed(leftMotor, leftMotorDirectionCtrl_1, leftMotorDirectionCtrl_2, leftSpeed);
    SetSpeed(rightMotor, rightMotorDirectionCtrl_1, rightMotorDirectionCtrl_2, rightSpeed);
}

void mBot::SetSpeed(PwmOut *motor, DigitalOut *directionCtrl_1, DigitalOut *directionCtrl_2, int speed)
{

    if (speed == 0)
    {
        directionCtrl_1->write(0);
        directionCtrl_2->write(1);   
        motor->write(0.0);
    }
    else if (speed < 0)
    {
        directionCtrl_1->write(0);
        directionCtrl_2->write(1);                
        motor->write(speed / -100.0);
    }
    else
    {
        directionCtrl_1->write(1);
        directionCtrl_2->write(0);   
        motor->write( speed / 100.0);
    }

}