![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
The preloaded firmware shipped on the mBot.
Please use the version of this project/repo found at: http://developer.mbed.org/teams/Outrageous-Circuits/code/Official_mBot/
Diff: mBot_lib/mBot.cpp
- Revision:
- 0:865d42c46692
diff -r 000000000000 -r 865d42c46692 mBot_lib/mBot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mBot_lib/mBot.cpp Tue Nov 25 14:34:47 2014 +0000 @@ -0,0 +1,139 @@ +/* +** 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); + } + +}