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