The preloaded firmware shipped on the mBot.
Dependencies: mbed
Fork of Official_mBot by
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?
User | Revision | Line number | New 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 | } |