Diff: main.cpp

Revision:

4:cae255669971

Parent:

3:402a6464f025

Child:

5:864709d3eb76

--- a/main.cpp	Mon Jun 25 00:56:05 2018 +0000
+++ b/main.cpp	Fri Jun 29 02:30:38 2018 +0000
@@ -25,7 +25,7 @@
 +==============================================*/
 
 // ROS
-ros::NodeHandle nh; // Comment out during LED tests?
+ros::NodeHandle nh; // Comment out during serial tests
 
 /*=============================================+
 |   ENUMERATIONS
@@ -81,7 +81,7 @@
 
 //#####################################################################################\\
 
-/*Serial pc(USBTX, USBRX);
+/*
 void printBits(char myByte){
  for (char mask = 0x80; mask; mask >>= 1){
    if(mask  & myByte)
@@ -108,6 +108,23 @@
 int main() {
 
 mbed::i2c.frequency(10000); // set required i2c frequency
+
+// Testing Wheel Encoder
+/*Serial pc(USBTX, USBRX); // Comment out during normal operation
+pc.printf("Hello World!\n");
+QEI wheel_encoder(PIN_WENCA, PIN_WENCB, NC, NCREV, SPEED_COUNTS, QEI::X4_ENCODING);
+Timer t_QEI;
+t_QEI.reset();
+t_QEI.start();
+while (true) {
+    if (t_QEI.read_ms() >= 500) {    
+        t_QEI.reset();
+        pc.printf("Current number of counts: %d\n", wheel_encoder.getPulses());
+        pc.printf("Measured wheel speed: %f\n", -wheel_encoder.getSpeed()*COUNTS2SHAFT);
+    }
+}*/
+    
+
 ////////
 // Blinky test code
 /*    nh.getHardware()->setBaud(115200);
@@ -214,6 +231,7 @@
 /******************************
     Normal code
 *******************************/
+
     bool verbose = true;            // Toggle debug topic output
     Timer t_LED, t_RGB_LED, t_PID, t_thrust, t_meas, t_pid_debug;   // object to do timing to compute motor speed
     
@@ -224,7 +242,7 @@
     
     FreeFlyerHardware freeflyer(nh, &mbed::i2c, mbed::thruster_pinouts,
         &mbed::led_inv_out_en, verbose);
-    
+
     t_LED.reset();
     t_RGB_LED.reset();
     t_PID.reset();
@@ -324,6 +342,7 @@
         }
 
         nh.spinOnce();
+        
     }
 
 }
\ No newline at end of file