Jesus Fausto
a
Diff: main.cpp
- Revision:
- 10:347b11f2e19c
- Parent:
- 9:ca11e4db63a7
--- a/main.cpp Fri Apr 19 23:08:27 2019 +0000
+++ b/main.cpp Tue Jun 25 17:55:17 2019 +0000
@@ -1,29 +1,30 @@
#include "wheelchair.h"
-QEI wheel (D10, D9, NC, 1200); //Initializes right encoder
-DigitalIn pt3(D10, PullUp); //Pull up resistors to read analog signals into digital signals
+/* Intializes the right encoder */
+QEI wheelS(D9, D10, NC, 1200);
+/* Set pull-up resistors to read analog signals into digital signals */
+DigitalIn pt3(D10, PullUp);
DigitalIn pt4(D9, PullUp);
-/*added*/
-//DigitalIn e_button(D4); //emergency button will start at HIGH
-
-QEI wheelS (D7, D8, NC, 1200); //Initializes Left encoder
-DigitalIn pt1(D7, PullUp); //Pull up resistors to read analog signals into digital signals
+/* Initializes Left encoder */
+QEI wheel (D7, D8, NC, 1200);
+/* Set pull-up resistors to read analog signals into digital signals */
+DigitalIn pt1(D7, PullUp);
DigitalIn pt2(D8, PullUp);
-int max_velocity;
-//Timer testAccT;
-
-AnalogIn x(A0); //Initializes analog axis for the joystick
+/* Initializes analog axis for the joystick */
+AnalogIn x(A0);
AnalogIn y(A1);
-DigitalOut up(D12); //Turn up speed mode for joystick
-DigitalOut down(D13); //Turn down speed mode for joystick
-DigitalOut on(D14); //Turn Wheelchair On
-DigitalOut off(D15); //Turn Wheelchair Off
-bool manual = false; //Turns chair joystic to automatic and viceverza
+double test1;
+double test2;
-Serial pc(USBTX, USBRX, 57600); //Serial Monitor
+/* Initializing more pins for wheelchair control */
+DigitalOut up(D2); //Turn up speed mode for joystick
+DigitalOut down(D8); //Turn down speed mode for joystick
+DigitalOut on(D12); //Turn Wheelchair On
+DigitalOut off(D11); //Turn Wheelchair Off
+
VL53L1X sensor1(PB_11, PB_10, D0); //initializes ToF sensors
VL53L1X sensor2(PB_11, PB_10, D1);
@@ -42,98 +43,116 @@
&sensor7, &sensor8, &sensor9, &sensor10, &sensor11, &sensor12}; //puts ToF sensor pointers into an array
VL53L1X** ToFT = ToF;
-Timer t; //Initialize time object t
-EventQueue queue; //Class to organize threads
+/* Changes the control mode of wheelchair: Automatic or Manual */
+bool manual = false;
+
+Serial pc(USBTX, USBRX, 57600); //Serial Monitor
+Timer t; //Initialize time object t
+EventQueue queue; //Class to organize threads
+
+ros::NodeHandle nh;
+nav_msgs::Odometry odom;
+geometry_msgs::Twist commandRead;
+
+void handlerFunction(const geometry_msgs::Twist& command){
+ commandRead = command;
+}
+
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
+ros::Publisher chatter("odom", &odom);
+ros::Publisher chatter2("cmd_vel", &commandRead);
+
+/* Initialize Wheelchair objects and threads */
Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS, ToFT); //Initialize wheelchair object
-Thread compass; //Thread for compass
-Thread velocity; //Thread for velosity
-Thread assistSafe; //thread for safety stuff
+Thread compass;
+Thread velocity;
+Thread ros_com;
+Thread assistSafe;
+/* This thread continues the communication with ROS and Mbed */
+void rosCom_thread();
int main(void)
{
-/* nh.initNode();
+ /* Initialize ROS commands to publish and subscribe to nodes */
+ nh.initNode();
nh.advertise(chatter);
nh.advertise(chatter2);
- nh.subscribe(sub);*/
- //testAccT.start();
- pc.printf("before starting\r\n");
- queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread
- queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread); //Sets up sampling frequency of the velosity_thread
+ nh.subscribe(sub);
+
+ /* Sets up sampling frequency of threads */
+ queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
+ queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velocity_thread);
queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::assistSafe_thread); //Sets up sampling frequency of the velosity_thread
- //queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread
- t.reset();
- compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread
- velocity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
+ queue.call_every(200, rosCom_thread);
+
+ t.reset(); //resets the time
+
+ /* Start running threads */
+ compass.start(callback(&queue, &EventQueue::dispatch_forever));
+ velocity.start(callback(&queue, &EventQueue::dispatch_forever));
assistSafe.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
- //ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
- pc.printf("After starting\r\n");
-
- //added
- // int emerg_button = e_button;
-
- int set = 0;
+ ros_com.start(callback(&queue, &EventQueue::dispatch_forever));
+
+
while(1) {
- if( pc.readable()) {
- set = 1;
- char c = pc.getc(); //Read the instruction sent
- if( c == 'w') {
- smart.forward(); //Move foward
-
- }
- else if( c == 'a') {
- smart.left(); //Turn left
- }
- else if( c == 'd') {
- smart.right(); //Turn right
- }
- else if( c == 's') {
- smart.backward(); //Turn rackwards
- }
-
- else if( c == 't') {
- smart.pid_twistA();
- } else if(c == 'v'){
- smart.showOdom();
- } else if(c == 'o') { //Turns on chair
- pc.printf("turning on\r\n");
- on = 1;
- wait(1);
- on = 0;
- } else if(c == 'f') { //Turns off chair
- pc.printf("turning off\r\n");
- off = 1;
- wait(1);
- off = 0;
-
- } else if(c == 'k'){ //Sends command to go to the kitchen
- smart.pid_twistV();
- } else if( c == 'm' || manual) { //Turns wheelchair to joystick
- pc.printf("turning on joystick\r\n");
- manual = true;
- t.reset();
- while(manual) {
- smart.move(x,y); //Reads from joystick and moves
- if( pc.readable()) {
- char d = pc.getc();
- if( d == 'm') { //Turns wheelchair from joystick into auto
- pc.printf("turning off joystick\r\n");
- manual = false;
- }
- }
- }
- }
- else {
- pc.printf("none \r\n");
- smart.stop(); //If nothing else is happening stop the chair
- }
+ /* If Ros comands the wheelchair to move fowards or backwards*/
+ if(commandRead.linear.x != 0)
+ {
+ smart.pid_twistV(); //Updates the twist linear velocity of chair
+ test1 = 3.14159;
+ }
+ /* If Ros comands the wheelchair to turn at a certain speed*/
+ else if(commandRead.angular.z != 0)
+ {
+ smart.pid_twistA(); //Updates the twist angular velocity of chair
+ test2 = 2.782;
+ }
+ /* If Ros does not give veloc
+ ity comands*/
+ else
+ {
+ smart.stop(); //Stops the chair
+ test2 = 0;
+ test1 = 0;
}
- else {
-
- smart.stop(); //If nothing else is happening stop the chair
- }
-
- wait(process);
+ wait(process); //Delay
}
+
}
+/* This thread allows for continuous update and publishing of ROS Odometry/Twist message */
+void rosCom_thread()
+{
+ /*Determines linear and angular velocity */
+ smart.linearV = commandRead.linear.x;
+ smart.angularV = commandRead.angular.z*180/3.1415926;
+
+ /* Publishes the position of the Wheelchair for Odometry */
+ odom.pose.pose.position.x = smart.x_position;
+ odom.pose.pose.position.y = smart.y_position;
+ odom.pose.pose.position.z = 0;
+
+ /* Publishes the orientation of the Wheelchair for Odometry */
+ odom.pose.pose.orientation.z = sin(smart.z_angular*.5*3.1415926/180);
+ odom.pose.pose.orientation.x = 0;
+ odom.pose.pose.orientation.y = 0;
+ odom.pose.pose.orientation.w = cos(smart.z_angular*.5*3.1415926/180);
+
+ /* Publishes Twist linear velocity of Wheelchair */
+ odom.twist.twist.linear.x = smart.vel;
+ odom.twist.twist.linear.y = commandRead.linear.x;
+ odom.twist.twist.linear.z = commandRead.angular.z;
+
+ /* Publishes Twist angular velocity of Wheelchair */
+ odom.twist.twist.angular.x = test1;
+ odom.twist.twist.angular.y = test2;
+ odom.twist.twist.angular.z = smart.getTwistZ()*3.1415926/180;
+
+ /* Allows for Odometry to be published and sent to ROS */
+ chatter.publish(&odom);
+ //chatter2.publish(&commandRead);
+
+ /*Checks for incoming messages */
+ nh.spinOnce();
+}
\ No newline at end of file