Fa Cake / Mbed OS mbed-os-example-blinky

my

Dependencies:   QEI BNO055 ros_lib_kinetic

main.cpp@0:ae2e2cabc497, 2019-08-01 (annotated)

Committer:
fllay
Date:
Thu Aug 01 17:10:38 2019 +0000
Revision:
0:ae2e2cabc497
fa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fllay 0:ae2e2cabc497 1 /* mbed Microcontroller Library
fllay 0:ae2e2cabc497 2 * Copyright (c) 2018 ARM Limited
fllay 0:ae2e2cabc497 3 * SPDX-License-Identifier: Apache-2.0
fllay 0:ae2e2cabc497 4 */
fllay 0:ae2e2cabc497 5
fllay 0:ae2e2cabc497 6 #include "mbed.h"
fllay 0:ae2e2cabc497 7 #include "QEI.h"
fllay 0:ae2e2cabc497 8 #include "ros.h"
fllay 0:ae2e2cabc497 9
fllay 0:ae2e2cabc497 10 #include "geometry_msgs/Twist.h"
fllay 0:ae2e2cabc497 11 ros::NodeHandle nh;
fllay 0:ae2e2cabc497 12
fllay 0:ae2e2cabc497 13 Serial pc(USBTX, USBRX);
fllay 0:ae2e2cabc497 14 DigitalOut led1(PA_10);
fllay 0:ae2e2cabc497 15 DigitalOut M1_dir(PA_8);
fllay 0:ae2e2cabc497 16 DigitalOut M2_dir(PB_10);
fllay 0:ae2e2cabc497 17 PwmOut PWM1(PB_4);
fllay 0:ae2e2cabc497 18 PwmOut PWM2(PB_5);
fllay 0:ae2e2cabc497 19
fllay 0:ae2e2cabc497 20 QEI rightWheel (PA_5, PA_6, NC, 624);
fllay 0:ae2e2cabc497 21 QEI leftWheel (PA_7, PB_6, NC, 624);
fllay 0:ae2e2cabc497 22
fllay 0:ae2e2cabc497 23 void commandCallback(const geometry_msgs::Twist& cmd_msg);
fllay 0:ae2e2cabc497 24 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
fllay 0:ae2e2cabc497 25
fllay 0:ae2e2cabc497 26 #define SLEEP_TIME 500 // (msec)
fllay 0:ae2e2cabc497 27 #define PRINT_AFTER_N_LOOPS 20
fllay 0:ae2e2cabc497 28
fllay 0:ae2e2cabc497 29 // main() runs in its own thread in the OS
fllay 0:ae2e2cabc497 30 int main()
fllay 0:ae2e2cabc497 31 {
fllay 0:ae2e2cabc497 32 PWM1.period(0.01);
fllay 0:ae2e2cabc497 33 PWM1 = 0.5;
fllay 0:ae2e2cabc497 34 PWM2.period(0.01);
fllay 0:ae2e2cabc497 35 PWM2 = 0.5;
fllay 0:ae2e2cabc497 36 M1_dir = 1;
fllay 0:ae2e2cabc497 37 M2_dir = 1;
fllay 0:ae2e2cabc497 38 while (true)
fllay 0:ae2e2cabc497 39 {
fllay 0:ae2e2cabc497 40 led1 = !led1;
fllay 0:ae2e2cabc497 41 wait_ms(SLEEP_TIME);
fllay 0:ae2e2cabc497 42 pc.printf("Right Pulses is: %i Left pulses is %i\n", rightWheel.getPulses(), leftWheel.getPulses());
fllay 0:ae2e2cabc497 43 rightWheel.reset();
fllay 0:ae2e2cabc497 44 leftWheel.reset();
fllay 0:ae2e2cabc497 45 }
fllay 0:ae2e2cabc497 46 }
fllay 0:ae2e2cabc497 47
fllay 0:ae2e2cabc497 48 void commandCallback(const geometry_msgs::Twist& cmd_msg)
fllay 0:ae2e2cabc497 49 {
fllay 0:ae2e2cabc497 50
fllay 0:ae2e2cabc497 51 }