VNG bot battle
Dependencies: BLE_API mbed nRF51822
Diff: MotorController.cpp
- Revision:
- 0:122d7bf3bbf0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorController.cpp Mon Sep 05 19:55:23 2016 +0000 @@ -0,0 +1,92 @@ +/* + * MotorController.cpp + * + * Created on: Aug 23, 2016 + * Author: tanpt + */ + +#include "MotorController.h" + +DigitalOut MotorController::motorLeftDirection_(p25); +PwmOut MotorController::motorLeftSpeed_(p23); +DigitalOut MotorController::motorRightDirection_(p28); +PwmOut MotorController::motorRightSpeed_(p24); + +DigitalOut MotorController::motorBoostSpeed_(p20); + +MotorController::MotorController() { + motorRightDirection_ = DIRECTION_FORWARD; + motorRightSpeed_.period_ms(1000); + motorLeftDirection_ = DIRECTION_FORWARD; + motorLeftSpeed_.period_ms(1000); +} + +MotorController::~MotorController() { + +} + +int8_t MotorController::setMotorRight(Direction direction, uint16_t speed) { + motorRightDirection_ = direction; + motorRightSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::setMotorLeft(Direction direction, uint16_t speed) { + motorLeftDirection_ = direction; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::moveForward(uint16_t speed) { + motorRightDirection_ = DIRECTION_FORWARD; + motorRightSpeed_.pulsewidth_ms(speed); + motorLeftDirection_ = DIRECTION_FORWARD; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::moveBackward(uint16_t speed) { + motorRightDirection_ = DIRECTION_BACKWARD; + motorRightSpeed_.pulsewidth_ms(speed); + motorLeftDirection_ = DIRECTION_BACKWARD; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::turnRight(uint16_t speed) { + motorRightDirection_ = DIRECTION_BACKWARD; + motorRightSpeed_.pulsewidth_ms(speed); + motorLeftDirection_ = DIRECTION_FORWARD; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::turnLeft(uint16_t speed) { + motorRightDirection_ = DIRECTION_FORWARD; + motorRightSpeed_.pulsewidth_ms(speed); + motorLeftDirection_ = DIRECTION_BACKWARD; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::moveBackRight(uint16_t speed) { + motorRightDirection_ = DIRECTION_BACKWARD; + motorRightSpeed_.pulsewidth_ms(speed); + motorLeftDirection_ = DIRECTION_BACKWARD; + motorLeftSpeed_.pulsewidth_ms(speed / 3); + return 0; + +} + +int8_t MotorController::moveBackLeft(uint16_t speed) { + motorRightDirection_ = DIRECTION_BACKWARD; + motorRightSpeed_.pulsewidth_ms(speed / 3); + motorLeftDirection_ = DIRECTION_BACKWARD; + motorLeftSpeed_.pulsewidth_ms(speed); + return 0; +} + +int8_t MotorController::setBoost(BoostState state) { + motorBoostSpeed_ = state; +} +