Diff: Command.cpp

Revision:

1:13c4bf8989d5

Parent:

0:4bc34a5fcc29

Child:

2:3d1c13d63966

--- a/Command.cpp	Sat Nov 21 17:17:41 2015 +0000
+++ b/Command.cpp	Sun Nov 22 01:04:17 2015 +0000
@@ -1,32 +1,66 @@
 #pragma once
 #include "Command.h"
 #include "mbed.h"
+#include "Motor.h"
 
-static DigitalOut led1(LED1);
+Motor _RIGHT_WHEEL(p25, p6, p5); // Motor A pwm, fwd, rev
+Motor _LEFT_WHEEL(p26, p7, p8);//Motor B pwm, fwd, rev
+DigitalOut _STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. 
+AnalogIn _IR(p17); //IR sensor
+DigitalOut _LED1(LED1);
+
+DigitalIn _LEFT_ENCODER(p19);
+DigitalIn _RIGHT_ENCODER(p20);
 
+Serial pc(USBTX,USBRX);
+
+Command::Command():_LEFT_SPEED(0),_RIGHT_SPEED(0)
+{}    
+
+void Command::setSpeed(const std::int8_t& l_sp,const std::int8_t& r_sp)
+{
+    _STBY = 1;
+    _LEFT_SPEED = l_sp;
+    _RIGHT_SPEED = r_sp;
+}
 
 void LedCommand :: execute()
 {
-    led1 = !led1;
+    _LED1 = !_LED1;
 }
 
 void TurnLeftCommand :: execute()
 {
+    _STBY = 1;
+    _RIGHT_WHEEL.speed(_RIGHT_SPEED);
+    _LEFT_WHEEL.speed(_LEFT_SPEED);
 }
 
 void TurnRightCommand :: execute()
 {
+    _STBY = 1;
+    _RIGHT_WHEEL.speed(_RIGHT_SPEED);
+    _LEFT_WHEEL.speed(_LEFT_SPEED);
 }
 
 void MoveForwardCommand :: execute()
 {
+    _STBY = 1;
+    _RIGHT_WHEEL.speed(0.5);
+    _LEFT_WHEEL.speed(0.5);
 }
 
 void MoveBackwardCommand :: execute()
 {
+
+    _STBY = 1;
+    _RIGHT_WHEEL.speed(_RIGHT_SPEED);
+    _LEFT_WHEEL.speed(_LEFT_SPEED);
 }
 
 void StopCommand :: execute()
 { 
+    _RIGHT_WHEEL.speed(_RIGHT_SPEED);
+    _LEFT_WHEEL.speed(_LEFT_SPEED);
 }