encoder in robot class

Author: gbr1

Arduino_Robot_Firmware.h

@@ -17,6 +17,8 @@ class Arduino_Robot_Firmware{
         RGBled * led2;
         DCmotor * motor_left;
         DCmotor * motor_right;
+        Encoder * encoder_left;
+        Encoder * encoder_right;
 
 
         Arduino_Robot_Firmware(){
@@ -26,6 +28,9 @@ class Arduino_Robot_Firmware{
             motor_left = new DCmotor(MOTOR_LEFT_A,MOTOR_LEFT_A_CH, MOTOR_LEFT_B, MOTOR_LEFT_B_CH,true);
             motor_right = new DCmotor(MOTOR_RIGHT_A,MOTOR_RIGHT_A_CH,MOTOR_RIGHT_B,MOTOR_RIGHT_B_CH);
 
+            encoder_left = new Encoder(TIM3);
+            encoder_right = new Encoder(TIM5);
+
         }
 
         int begin(){
@@ -43,6 +48,9 @@ class Arduino_Robot_Firmware{
             motor_left->stop();
             motor_right->stop();
 
+            encoder_left->begin();
+            encoder_right->begin();
+
             return 0;
         }
 };

examples/simple/simple.ino

@@ -1,14 +1,33 @@
 #include "Arduino_Robot_Firmware.h"
 
+
 Arduino_Robot_Firmware robot;
 
 void setup(){
-    robot.begin();
+  Serial.begin(115200);
+  robot.begin();
 }
 
 void loop(){
-    robot.led1->setRed(1);
-    delay(1000);
-    robot.led1->setRed(0);
-    delay(1000);
+  robot.motor_left->setSpeed(4095);
+  robot.motor_right->setSpeed(4095);
+  Serial.print(robot.encoder_left->getCount());
+  Serial.print("\t");
+  Serial.print(robot.encoder_right->getCount());
+  Serial.print("\n"); 
+
+  robot.led1->set(HIGH,LOW,LOW);
+  robot.led2->set(LOW,LOW,HIGH);
+  delay(1000);
+
+  robot.motor_left->setSpeed(-4095);
+  robot.motor_right->setSpeed(4095);
+  Serial.print(robot.encoder_left->getCount());
+  Serial.print("\t");
+  Serial.print(robot.encoder_right->getCount());
+  Serial.print("\n"); 
+  
+  robot.led1->set(LOW,LOW,HIGH);
+  robot.led2->set(HIGH,LOW,LOW);
+  delay(1000);
 }