MotorDriver.cpp

00001 /*
00002   MotorDriver.cpp
00003   2014 Copyright (c) Seeed Technology Inc.  All right reserved.
00004 
00005   Author:lawliet.zou@gmail.com
00006   2014-02-11
00007 
00008   This library is free software; you can redistribute it and/or
00009   modify it under the terms of the GNU Lesser General Public
00010   License as published by the Free Software Foundation; either
00011   version 2.1 of the License, or (at your option) any later version.
00012 
00013   This library is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016   Lesser General Public License for more details.
00017 
00018   You should have received a copy of the GNU Lesser General Public
00019   License along with this library; if not, write to the Free Software
00020   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 */
00022 
00023 #include "MotorDriver.h"
00024 
00025 void MotorDriver::init()
00026 {
00027     stop();
00028     /*Configure the motor A to control the wheel at the left side.*/
00029     configure(MOTOR_POSITION_LEFT,MOTORA);
00030     /*Configure the motor B to control the wheel at the right side.*/
00031     configure(MOTOR_POSITION_RIGHT,MOTORB);
00032     setSpeed(0,MOTORA);
00033     setSpeed(0,MOTORB);
00034     setDirection(MOTOR_ANTICLOCKWISE,MOTORA);
00035     setDirection(MOTOR_CLOCKWISE,MOTORB);
00036 }
00037 void MotorDriver::configure(uint8_t position, uint8_t motorID)
00038 {
00039     if(motorID == MOTORA)motorA.position = position;
00040     else motorB.position = position;
00041 }
00042 
00043 void MotorDriver::setSpeed(uint8_t speed, uint8_t motorID)
00044 {
00045     if(motorID == MOTORA) motorA.speed = speed;
00046     else if(motorID == MOTORB) motorB.speed = speed;
00047 }
00048 void MotorDriver::setDirection(uint8_t direction, uint8_t motorID)
00049 {
00050     if(motorID == MOTORA)motorA.direction= direction;
00051     else if(motorID == MOTORB)motorB.direction = direction;
00052 }
00053 
00054 void MotorDriver::rotate(uint8_t direction, uint8_t motor_position)
00055 {
00056     if(motor_position == motorA.position) {
00057         rotateWithID(direction,MOTORA);
00058     }
00059     if(motor_position == motorB.position) {
00060         rotateWithID(direction,MOTORB);
00061     }
00062 }
00063 
00064 void MotorDriver::rotateWithID(uint8_t direction, uint8_t motorID)
00065 {
00066     if(motorID == MOTORA) {
00067         _speedA.Enable(motorA.speed*100,MOTOR_PERIOD);
00068         _int1 = (MOTOR_CLOCKWISE == direction)?LOW:HIGH;
00069         _int2 = !_int1;
00070     } else if(motorID == MOTORB) {
00071         _speedB.Enable(motorB.speed*100,MOTOR_PERIOD);
00072         _int3 = (MOTOR_CLOCKWISE == direction)?LOW:HIGH;
00073         _int4 = !_int3;
00074     }
00075 }
00076 
00077 void MotorDriver::goForward()
00078 {
00079     rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_LEFT);
00080     rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_RIGHT);
00081 }
00082 void MotorDriver::goBackward()
00083 {
00084     rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_RIGHT);
00085     rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_LEFT);
00086 }
00087 void MotorDriver::goLeft()
00088 {
00089     rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_RIGHT);
00090     rotate(MOTOR_CLOCKWISE,MOTOR_POSITION_LEFT);
00091 }
00092 void MotorDriver::goRight()
00093 {
00094     rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_RIGHT);
00095     rotate(MOTOR_ANTICLOCKWISE,MOTOR_POSITION_LEFT);
00096 }
00097 
00098 void MotorDriver::stop()
00099 {
00100     _speedA.Disable();
00101     _speedB.Disable();
00102 }
00103 
00104 void MotorDriver::stop(uint8_t motorID)
00105 {
00106     if(motorID == MOTORA)_speedA.Disable();
00107     else if(motorID == MOTORB)_speedB.Disable();
00108 }
00109