Diff: MotorDriver.cpp

Revision:

0:6bd25809e2e0

Child:

1:9c93f514f369

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver.cpp	Wed Feb 12 02:54:52 2014 +0000
@@ -0,0 +1,118 @@
+/*
+  MotorDriver.cpp
+  2014 Copyright (c) Seeed Technology Inc.  All right reserved.
+
+  Author:lawliet.zou@gmail.com
+  2014-02-11
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "MotorDriver.h"
+
+void MotorDriver::init()
+{
+    stop();
+    /*Configure the motor A to control the wheel at the left side.*/
+    configure(MOTOR_POSITION_LEFT,MOTORA);
+    /*Configure the motor B to control the wheel at the right side.*/
+    configure(MOTOR_POSITION_RIGHT,MOTORB);
+    setSpeed(0,MOTORA);
+    setSpeed(0,MOTORB);
+    setDirection(MOTOR_ANTICLOCKWISE,MOTORA);
+    setDirection(MOTOR_CLOCKWISE,MOTORB);
+}
+void MotorDriver::configure(uint8_t position, uint8_t motorID)
+{
+    if(motorID == MOTORA)motorA.position = position;
+    else motorB.position = position;
+}
+
+void MotorDriver::setSpeed(uint8_t speed, uint8_t motorID)
+{
+    if(motorID == MOTORA) motorA.speed = speed;
+    else if(motorID == MOTORB) motorB.speed = speed;
+}
+void MotorDriver::setDirection(uint8_t direction, uint8_t motorID)
+{
+    if(motorID == MOTORA)motorA.direction= direction;
+    else if(motorID == MOTORB)motorB.direction = direction;
+}
+/**********************************************************************/
+/*Function: Get the motor rotate                                      */
+/*Parameter:-uint8_t direction,Clockwise or anticlockwise;            */
+/*          -uint8_t motor_position,MOTOR_POSITION_LEFT or            */
+/*          MOTOR_POSITION_RIGHT;                                     */
+/*Return:   void                                                      */
+void MotorDriver::rotate(uint8_t direction, uint8_t motor_position)
+{
+    if(motor_position == motorA.position) {
+        rotateWithID(direction,MOTORA);
+    }
+    if(motor_position == motorB.position) {
+        rotateWithID(direction,MOTORB);
+    }
+}
+/**********************************************************************/
+/*Function: Get the motor rotate                                      */
+/*Parameter:-uint8_t direction,Clockwise or anticlockwise;            */
+/*          -uint8_t motor_position,MOTORA or MOTORB                  */
+/*Return:   void                                                      */
+void MotorDriver::rotateWithID(uint8_t direction, uint8_t motorID)
+{
+    if(motorID == MOTORA) {
+        _speedA.Enable(motorA.speed*100,MOTOR_PERIOD);
+        _int1 = (MOTOR_CLOCKWISE == direction)?LOW:HIGH;
+        _int2 = !_int1;
+    } else if(motorID == MOTORB) {
+        _speedB.Enable(motorB.speed*100,MOTOR_PERIOD);
+        _int3 = (MOTOR_CLOCKWISE == direction)?LOW:HIGH;
+        _int4 = !_int3;
+    }
+}
+
+void MotorDriver::goForward()
+{
+    rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_LEFT);
+    rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_RIGHT);
+}
+void MotorDriver::goBackward()
+{
+    rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_RIGHT);
+    rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_LEFT);
+}
+void MotorDriver::goLeft()
+{
+    rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_RIGHT);
+    rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_LEFT);
+}
+void MotorDriver::goRight()
+{
+    rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_RIGHT);
+    rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_LEFT);
+}
+
+void MotorDriver::stop()
+{
+    _speedA.Disable();
+    _speedB.Disable();
+}
+
+void MotorDriver::stop(uint8_t motorID)
+{
+    if(motorID == MOTORA)_speedA.Disable();
+    else if(motorID == MOTORB)_speedB.Disable();
+}
+