Diff: main.cpp

Revision:

0:ae2e2cabc497

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 01 17:10:38 2019 +0000
@@ -0,0 +1,51 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "QEI.h"
+#include "ros.h"
+
+#include "geometry_msgs/Twist.h"
+ros::NodeHandle  nh;
+
+Serial pc(USBTX, USBRX);
+DigitalOut led1(PA_10);
+DigitalOut M1_dir(PA_8);
+DigitalOut M2_dir(PB_10);
+PwmOut PWM1(PB_4);
+PwmOut PWM2(PB_5);
+
+QEI rightWheel (PA_5, PA_6, NC, 624);
+QEI leftWheel (PA_7, PB_6, NC, 624);
+
+void commandCallback(const geometry_msgs::Twist& cmd_msg);
+ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
+
+#define SLEEP_TIME                  500 // (msec)
+#define PRINT_AFTER_N_LOOPS         20
+
+// main() runs in its own thread in the OS
+int main()
+{
+    PWM1.period(0.01); 
+    PWM1 = 0.5;
+    PWM2.period(0.01); 
+    PWM2 = 0.5;
+    M1_dir = 1;
+    M2_dir = 1;
+    while (true)
+    {
+        led1 = !led1;
+        wait_ms(SLEEP_TIME);
+        pc.printf("Right Pulses is: %i Left pulses is %i\n", rightWheel.getPulses(), leftWheel.getPulses());
+        rightWheel.reset();
+        leftWheel.reset();
+    }
+}
+
+void commandCallback(const geometry_msgs::Twist& cmd_msg)
+{
+ 
+}