![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
my
Dependencies: QEI BNO055 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:ae2e2cabc497
diff -r 000000000000 -r ae2e2cabc497 main.cpp --- /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) +{ + +}