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