GHI Electronics / Mbed 2 deprecated Official_mBot

The preloaded firmware shipped on the mBot.

Dependencies:   mbed

Fork of Official_mBot by Fred Parker

mBot_lib/mBot.cpp@1:ffd9a51e7d35, 2014-11-25 (annotated)

Committer:
jeffknaggs
Date:
Tue Nov 25 14:49:40 2014 +0000
Revision:
1:ffd9a51e7d35
Parent:
0:865d42c46692 
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jeffknaggs 0:865d42c46692 1 /*
jeffknaggs 0:865d42c46692 2 ** This software can be freely used, even comercially, as highlighted in the license.
jeffknaggs 0:865d42c46692 3 **
jeffknaggs 0:865d42c46692 4 ** Copyright 2014 GHI Electronics, LLC
jeffknaggs 0:865d42c46692 5 **
jeffknaggs 0:865d42c46692 6 ** Licensed under the Apache License, Version 2.0 (the "License");
jeffknaggs 0:865d42c46692 7 ** you may not use this file except in compliance with the License.
jeffknaggs 0:865d42c46692 8 ** You may obtain a copy of the License at
jeffknaggs 0:865d42c46692 9 **
jeffknaggs 0:865d42c46692 10 ** http://www.apache.org/licenses/LICENSE-2.0
jeffknaggs 0:865d42c46692 11 **
jeffknaggs 0:865d42c46692 12 ** Unless required by applicable law or agreed to in writing, software
jeffknaggs 0:865d42c46692 13 ** distributed under the License is distributed on an "AS IS" BASIS,
jeffknaggs 0:865d42c46692 14 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jeffknaggs 0:865d42c46692 15 ** See the License for the specific language governing permissions and
jeffknaggs 0:865d42c46692 16 ** limitations under the License.
jeffknaggs 0:865d42c46692 17 **
jeffknaggs 0:865d42c46692 18 **/
jeffknaggs 0:865d42c46692 19 #include "mBot.h"
jeffknaggs 0:865d42c46692 20
jeffknaggs 0:865d42c46692 21 PwmOut *mBot::leftMotor = NULL;
jeffknaggs 0:865d42c46692 22 PwmOut *mBot::rightMotor = NULL;
jeffknaggs 0:865d42c46692 23
jeffknaggs 0:865d42c46692 24 DigitalOut *mBot::leftMotorDirectionCtrl_1 = NULL;
jeffknaggs 0:865d42c46692 25 DigitalOut *mBot::leftMotorDirectionCtrl_2 = NULL;
jeffknaggs 0:865d42c46692 26
jeffknaggs 0:865d42c46692 27 DigitalOut *mBot::rightMotorDirectionCtrl_1 = NULL;
jeffknaggs 0:865d42c46692 28 DigitalOut *mBot::rightMotorDirectionCtrl_2 = NULL;
jeffknaggs 0:865d42c46692 29
jeffknaggs 0:865d42c46692 30 bool mBot::leftMotorInverted = false;
jeffknaggs 0:865d42c46692 31 bool mBot::rightMotorInverted = false;
jeffknaggs 0:865d42c46692 32
jeffknaggs 0:865d42c46692 33 AnalogIn *mBot::leftSensor = NULL;
jeffknaggs 0:865d42c46692 34 AnalogIn *mBot::rightSensor = NULL;
jeffknaggs 0:865d42c46692 35 DigitalOut *mBot::leftIRLED = NULL;
jeffknaggs 0:865d42c46692 36 DigitalOut *mBot::rightIRLED = NULL;
jeffknaggs 0:865d42c46692 37
jeffknaggs 0:865d42c46692 38
jeffknaggs 0:865d42c46692 39 mBot::mBot()
jeffknaggs 0:865d42c46692 40 {
jeffknaggs 0:865d42c46692 41
jeffknaggs 0:865d42c46692 42 leftSensor = new AnalogIn(P0_14);
jeffknaggs 0:865d42c46692 43 rightSensor = new AnalogIn(P0_15);
jeffknaggs 0:865d42c46692 44
jeffknaggs 0:865d42c46692 45 leftIRLED = new DigitalOut(P0_23,1); // FIX: poweraving?
jeffknaggs 0:865d42c46692 46 rightIRLED = new DigitalOut(P0_17,1);
jeffknaggs 0:865d42c46692 47
jeffknaggs 0:865d42c46692 48 leftMotor = new PwmOut(P0_9);
jeffknaggs 0:865d42c46692 49 leftMotor->period(MOTOR_BASE_PERIOD); // float 1/frequency (seconds)
jeffknaggs 0:865d42c46692 50 leftMotor->write(0.5); // duty cycle
jeffknaggs 0:865d42c46692 51 leftMotorDirectionCtrl_1 = new DigitalOut(P0_7, 0);
jeffknaggs 0:865d42c46692 52 leftMotorDirectionCtrl_2 = new DigitalOut(P0_11, 0);
jeffknaggs 0:865d42c46692 53
jeffknaggs 0:865d42c46692 54
jeffknaggs 0:865d42c46692 55 rightMotor = new PwmOut(P0_8);
jeffknaggs 0:865d42c46692 56 rightMotor->period(MOTOR_BASE_PERIOD);
jeffknaggs 0:865d42c46692 57 rightMotor->write(0.5); // duty cycle
jeffknaggs 0:865d42c46692 58 rightMotorDirectionCtrl_1 = new DigitalOut(P0_12,0);
jeffknaggs 0:865d42c46692 59 rightMotorDirectionCtrl_2 = new DigitalOut(P0_13,0);
jeffknaggs 0:865d42c46692 60
jeffknaggs 0:865d42c46692 61
jeffknaggs 0:865d42c46692 62 leftMotorInverted = 0;
jeffknaggs 0:865d42c46692 63 rightMotorInverted = 0;
jeffknaggs 0:865d42c46692 64
jeffknaggs 0:865d42c46692 65 }
jeffknaggs 0:865d42c46692 66
jeffknaggs 0:865d42c46692 67
jeffknaggs 0:865d42c46692 68 float mBot::GetReflectiveReading(ReflectiveSensors sensor)
jeffknaggs 0:865d42c46692 69 {
jeffknaggs 0:865d42c46692 70 return sensor == Left ? leftSensor->read() : rightSensor->read();
jeffknaggs 0:865d42c46692 71 }
jeffknaggs 0:865d42c46692 72
jeffknaggs 0:865d42c46692 73 void mBot::SetReflectiveSensorState(int state)
jeffknaggs 0:865d42c46692 74 {
jeffknaggs 0:865d42c46692 75 leftIRLED->write(state);
jeffknaggs 0:865d42c46692 76 rightIRLED->write(state);
jeffknaggs 0:865d42c46692 77 }
jeffknaggs 0:865d42c46692 78
jeffknaggs 0:865d42c46692 79 void mBot::SetMotorInversion(bool invertLeft, bool invertRight)
jeffknaggs 0:865d42c46692 80 {
jeffknaggs 0:865d42c46692 81 leftMotorInverted = invertLeft;
jeffknaggs 0:865d42c46692 82 rightMotorInverted = invertRight;
jeffknaggs 0:865d42c46692 83 }
jeffknaggs 0:865d42c46692 84
jeffknaggs 0:865d42c46692 85 void mBot::SetMotorSpeed(int leftSpeed, int rightSpeed)
jeffknaggs 0:865d42c46692 86 {
jeffknaggs 0:865d42c46692 87 if (leftSpeed > 100 || leftSpeed < -100 || rightSpeed > 100 || rightSpeed < -100)
jeffknaggs 0:865d42c46692 88 {
jeffknaggs 0:865d42c46692 89 return;
jeffknaggs 0:865d42c46692 90 }
jeffknaggs 0:865d42c46692 91
jeffknaggs 0:865d42c46692 92 if ((leftSpeed | rightSpeed) == 0) {
jeffknaggs 0:865d42c46692 93 leftMotorDirectionCtrl_1->write(0);
jeffknaggs 0:865d42c46692 94 leftMotorDirectionCtrl_2->write(0);
jeffknaggs 0:865d42c46692 95 rightMotorDirectionCtrl_1->write(0);
jeffknaggs 0:865d42c46692 96 rightMotorDirectionCtrl_2->write(0);
jeffknaggs 0:865d42c46692 97 return;
jeffknaggs 0:865d42c46692 98 };
jeffknaggs 0:865d42c46692 99
jeffknaggs 0:865d42c46692 100
jeffknaggs 0:865d42c46692 101
jeffknaggs 0:865d42c46692 102 if (leftMotorInverted)
jeffknaggs 0:865d42c46692 103 {
jeffknaggs 0:865d42c46692 104 leftSpeed *= -1;
jeffknaggs 0:865d42c46692 105 }
jeffknaggs 0:865d42c46692 106
jeffknaggs 0:865d42c46692 107 if (rightMotorInverted)
jeffknaggs 0:865d42c46692 108 {
jeffknaggs 0:865d42c46692 109 rightSpeed *= -1;
jeffknaggs 0:865d42c46692 110 }
jeffknaggs 0:865d42c46692 111
jeffknaggs 0:865d42c46692 112
jeffknaggs 0:865d42c46692 113 SetSpeed(leftMotor, leftMotorDirectionCtrl_1, leftMotorDirectionCtrl_2, leftSpeed);
jeffknaggs 0:865d42c46692 114 SetSpeed(rightMotor, rightMotorDirectionCtrl_1, rightMotorDirectionCtrl_2, rightSpeed);
jeffknaggs 0:865d42c46692 115 }
jeffknaggs 0:865d42c46692 116
jeffknaggs 0:865d42c46692 117 void mBot::SetSpeed(PwmOut *motor, DigitalOut *directionCtrl_1, DigitalOut *directionCtrl_2, int speed)
jeffknaggs 0:865d42c46692 118 {
jeffknaggs 0:865d42c46692 119
jeffknaggs 0:865d42c46692 120 if (speed == 0)
jeffknaggs 0:865d42c46692 121 {
jeffknaggs 0:865d42c46692 122 directionCtrl_1->write(0);
jeffknaggs 0:865d42c46692 123 directionCtrl_2->write(1);
jeffknaggs 0:865d42c46692 124 motor->write(0.0);
jeffknaggs 0:865d42c46692 125 }
jeffknaggs 0:865d42c46692 126 else if (speed < 0)
jeffknaggs 0:865d42c46692 127 {
jeffknaggs 0:865d42c46692 128 directionCtrl_1->write(0);
jeffknaggs 0:865d42c46692 129 directionCtrl_2->write(1);
jeffknaggs 0:865d42c46692 130 motor->write(speed / -100.0);
jeffknaggs 0:865d42c46692 131 }
jeffknaggs 0:865d42c46692 132 else
jeffknaggs 0:865d42c46692 133 {
jeffknaggs 0:865d42c46692 134 directionCtrl_1->write(1);
jeffknaggs 0:865d42c46692 135 directionCtrl_2->write(0);
jeffknaggs 0:865d42c46692 136 motor->write( speed / 100.0);
jeffknaggs 0:865d42c46692 137 }
jeffknaggs 0:865d42c46692 138
jeffknaggs 0:865d42c46692 139 }