Jesus Fausto / Mbed OS wheelchaircontrolrealtime1

with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@10:beaa2ddfef32, 2018-11-03 (annotated)

Committer:
jvfausto
Date:
Sat Nov 03 20:36:36 2018 +0000
Revision:
10:beaa2ddfef32
Parent:
9:1081ebfe3db0 
Now with twist and Odom messages

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
ryanlin97 0:7e6b349182bc 2
jvfausto 9:1081ebfe3db0 3 QEI wheel(D0, D1, NC, 1200); //Initializes right encoder
jvfausto 9:1081ebfe3db0 4 DigitalIn pt3(D1, PullUp); //Pull up resistors to read analog signals into digital signals
jvfausto 8:db780b392bae 5 DigitalIn pt4(D0, PullUp);
jvfausto 8:db780b392bae 6
jvfausto 9:1081ebfe3db0 7 QEI wheelS (D3, D6, NC, 1200); //Initializes Left encoder
jvfausto 9:1081ebfe3db0 8 DigitalIn pt1(D3, PullUp); //Pull up resistors to read analog signals into digital signals
jvfausto 9:1081ebfe3db0 9 DigitalIn pt2(D6, PullUp);
jvfausto 8:db780b392bae 10
jvfausto 9:1081ebfe3db0 11 AnalogIn x(A0); //Initializes analog axis for the joystick
ryanlin97 0:7e6b349182bc 12 AnalogIn y(A1);
ryanlin97 0:7e6b349182bc 13
jvfausto 9:1081ebfe3db0 14 DigitalOut up(D2); //Turn up speed mode for joystick
jvfausto 9:1081ebfe3db0 15 DigitalOut down(D8); //Turn down speed mode for joystick
jvfausto 9:1081ebfe3db0 16 DigitalOut on(D12); //Turn Wheelchair On
jvfausto 9:1081ebfe3db0 17 DigitalOut off(D11); //Turn Wheelchair Off
jvfausto 9:1081ebfe3db0 18 bool manual = false; //Turns chair joystic to automatic and viceverza
ryanlin97 0:7e6b349182bc 19
jvfausto 9:1081ebfe3db0 20 Serial pc(USBTX, USBRX, 57600); //Serial Monitor
jvfausto 9:1081ebfe3db0 21
jvfausto 9:1081ebfe3db0 22 Timer t; //Initialize time object t
jvfausto 9:1081ebfe3db0 23 EventQueue queue; //Class to organize threads
ryanlin97 5:90bf5f0d86e9 24
jvfausto 9:1081ebfe3db0 25 ros::NodeHandle nh;
jvfausto 9:1081ebfe3db0 26 nav_msgs::Odometry odom;
jvfausto 10:beaa2ddfef32 27 geometry_msgs::Twist commandRead;
ryanlin97 0:7e6b349182bc 28
jvfausto 10:beaa2ddfef32 29 void handlerFunction(const geometry_msgs::Twist& command){
jvfausto 10:beaa2ddfef32 30 commandRead = command;
jvfausto 10:beaa2ddfef32 31 }
jvfausto 10:beaa2ddfef32 32
jvfausto 10:beaa2ddfef32 33 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &handlerFunction);
jvfausto 10:beaa2ddfef32 34 ros::Publisher chatter("odom", &odom);
jvfausto 10:beaa2ddfef32 35 ros::Publisher chatter2("cmd_vel", &commandRead);
ryanlin97 0:7e6b349182bc 36
jvfausto 9:1081ebfe3db0 37 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS); //Initialize wheelchair object
jvfausto 9:1081ebfe3db0 38 Thread compass; //Thread for compass
jvfausto 9:1081ebfe3db0 39 Thread velosity; //Thread for velosity
jvfausto 9:1081ebfe3db0 40 Thread ros_com; //Thread for velosity
ryanlin97 5:90bf5f0d86e9 41
jvfausto 10:beaa2ddfef32 42 void rosCom_thread()
jvfausto 10:beaa2ddfef32 43 {
jvfausto 10:beaa2ddfef32 44 odom.pose.pose.position.x = smart.x_position;
jvfausto 10:beaa2ddfef32 45 odom.pose.pose.position.y = smart.y_position;
jvfausto 10:beaa2ddfef32 46 odom.pose.pose.position.z = 0;
jvfausto 10:beaa2ddfef32 47 //set the orientation
jvfausto 10:beaa2ddfef32 48 odom.pose.pose.orientation.z = smart.z_angular;
jvfausto 10:beaa2ddfef32 49 odom.pose.pose.orientation.x = 0;
jvfausto 10:beaa2ddfef32 50 odom.pose.pose.orientation.y = 0;
jvfausto 10:beaa2ddfef32 51 odom.twist.twist.linear.x = smart.curr_vel;
jvfausto 10:beaa2ddfef32 52 odom.twist.twist.linear.y = 0;
jvfausto 10:beaa2ddfef32 53 odom.twist.twist.linear.z = 0;
jvfausto 10:beaa2ddfef32 54 odom.twist.twist.angular.x = 0;
jvfausto 10:beaa2ddfef32 55 odom.twist.twist.angular.y = 0;
jvfausto 10:beaa2ddfef32 56 odom.twist.twist.angular.z = smart.getTwistZ();
jvfausto 10:beaa2ddfef32 57 chatter.publish(&odom);
jvfausto 10:beaa2ddfef32 58 chatter2.publish(&commandRead);
jvfausto 10:beaa2ddfef32 59 nh.spinOnce();
jvfausto 10:beaa2ddfef32 60 }
ryanlin97 0:7e6b349182bc 61 int main(void)
jvfausto 9:1081ebfe3db0 62 {
jvfausto 9:1081ebfe3db0 63 nh.initNode();
jvfausto 9:1081ebfe3db0 64 nh.advertise(chatter);
jvfausto 10:beaa2ddfef32 65 nh.advertise(chatter2);
jvfausto 10:beaa2ddfef32 66 nh.subscribe(sub);
jvfausto 9:1081ebfe3db0 67
jvfausto 9:1081ebfe3db0 68 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread
jvfausto 9:1081ebfe3db0 69 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velosity_thread); //Sets up sampling frequency of the velosity_thread
jvfausto 10:beaa2ddfef32 70 queue.call_every(200, rosCom_thread); //Sets up sampling frequency of the velosity_thread
ryanlin97 0:7e6b349182bc 71 t.reset();
jvfausto 9:1081ebfe3db0 72 compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread
jvfausto 9:1081ebfe3db0 73 velosity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
jvfausto 9:1081ebfe3db0 74 ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
ryanlin97 0:7e6b349182bc 75 while(1) {
ryanlin97 0:7e6b349182bc 76 if( pc.readable()) {
jvfausto 9:1081ebfe3db0 77 char c = pc.getc(); //Read the instruction sent
ryanlin97 0:7e6b349182bc 78 if( c == 'w') {
jvfausto 9:1081ebfe3db0 79 smart.forward(); //Move foward
ryanlin97 0:7e6b349182bc 80 }
ryanlin97 0:7e6b349182bc 81 else if( c == 'a') {
jvfausto 9:1081ebfe3db0 82 smart.left(); //Turn left
ryanlin97 0:7e6b349182bc 83 }
ryanlin97 0:7e6b349182bc 84 else if( c == 'd') {
jvfausto 9:1081ebfe3db0 85 smart.right(); //Turn right
ryanlin97 0:7e6b349182bc 86 }
ryanlin97 0:7e6b349182bc 87 else if( c == 's') {
jvfausto 9:1081ebfe3db0 88 smart.backward(); //Turn rackwards
ryanlin97 0:7e6b349182bc 89 }
jvfausto 9:1081ebfe3db0 90
jvfausto 9:1081ebfe3db0 91 else if( c == 't') {
jvfausto 9:1081ebfe3db0 92 smart.pid_twistA();
jvfausto 9:1081ebfe3db0 93 /*
jvfausto 9:1081ebfe3db0 94 char buffer[256]; //Buffer for Angle to turn
jvfausto 9:1081ebfe3db0 95 pc.printf ("Enter a long number: ");
jvfausto 8:db780b392bae 96 int angle = 0;
jvfausto 9:1081ebfe3db0 97
jvfausto 9:1081ebfe3db0 98 //if you can not send a string
jvfausto 9:1081ebfe3db0 99 char d = pc.getc(); //Reads wether to turn right or left
jvfausto 8:db780b392bae 100 if(d == 'r')
jvfausto 8:db780b392bae 101 {
jvfausto 9:1081ebfe3db0 102 angle = 90;
jvfausto 8:db780b392bae 103 }
jvfausto 8:db780b392bae 104 if(d == 'l')
jvfausto 8:db780b392bae 105 {
jvfausto 9:1081ebfe3db0 106 angle = -90;
jvfausto 8:db780b392bae 107 }
jvfausto 9:1081ebfe3db0 108
jvfausto 9:1081ebfe3db0 109 //if you can send a string
jvfausto 9:1081ebfe3db0 110 /*
jvfausto 9:1081ebfe3db0 111 fgets (buffer, 256, stdin); //Reads string and puts it in a buffer
jvfausto 9:1081ebfe3db0 112 angle = atoi(buffer); //Converst string into integer
jvfausto 9:1081ebfe3db0 113 */
jvfausto 9:1081ebfe3db0 114 /*
ryanlin97 0:7e6b349182bc 115 if(angle == 0) {
jvfausto 9:1081ebfe3db0 116 pc.printf("invalid input try again\r\n");
ryanlin97 0:7e6b349182bc 117 } else {
jvfausto 9:1081ebfe3db0 118 smart.pid_turn(angle); //Sends instruction to turn desired angle
jvfausto 9:1081ebfe3db0 119 }*/
jvfausto 9:1081ebfe3db0 120 } else if(c == 'v'){
jvfausto 9:1081ebfe3db0 121 smart.showOdom();
jvfausto 9:1081ebfe3db0 122 } else if(c == 'o') { //Turns on chair
jvfausto 7:04f93e6b929f 123 pc.printf("turning on\r\n");
ryanlin97 6:e9b1684a9c00 124 on = 1;
ryanlin97 6:e9b1684a9c00 125 wait(1);
ryanlin97 6:e9b1684a9c00 126 on = 0;
jvfausto 9:1081ebfe3db0 127 } else if(c == 'f') { //Turns off chair
jvfausto 7:04f93e6b929f 128 pc.printf("turning off\r\n");
ryanlin97 6:e9b1684a9c00 129 off = 1;
ryanlin97 6:e9b1684a9c00 130 wait(1);
ryanlin97 6:e9b1684a9c00 131 off = 0;
jvfausto 9:1081ebfe3db0 132
jvfausto 9:1081ebfe3db0 133 } else if(c == 'k'){ //Sends command to go to the kitchen
jvfausto 9:1081ebfe3db0 134 smart.pid_twistV();
jvfausto 9:1081ebfe3db0 135 } else if( c == 'm' || manual) { //Turns wheelchair to joystick
jvfausto 7:04f93e6b929f 136 pc.printf("turning on joystick\r\n");
ryanlin97 0:7e6b349182bc 137 manual = true;
ryanlin97 0:7e6b349182bc 138 t.reset();
ryanlin97 0:7e6b349182bc 139 while(manual) {
jvfausto 9:1081ebfe3db0 140 smart.move(x,y); //Reads from joystick and moves
ryanlin97 0:7e6b349182bc 141 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 142 char d = pc.getc();
jvfausto 9:1081ebfe3db0 143 if( d == 'm') { //Turns wheelchair from joystick into auto
jvfausto 7:04f93e6b929f 144 pc.printf("turning off joystick\r\n");
ryanlin97 0:7e6b349182bc 145 manual = false;
ryanlin97 0:7e6b349182bc 146 }
ryanlin97 0:7e6b349182bc 147 }
jvfausto 7:04f93e6b929f 148 }
jvfausto 9:1081ebfe3db0 149 } else {
jvfausto 9:1081ebfe3db0 150 pc.printf("none \r\n");
jvfausto 9:1081ebfe3db0 151 smart.stop(); //If nothing else is happening stop the chair
ryanlin97 0:7e6b349182bc 152 }
ryanlin97 0:7e6b349182bc 153 }
ryanlin97 0:7e6b349182bc 154 else {
jvfausto 9:1081ebfe3db0 155 smart.stop(); //If nothing else is happening stop the chair
ryanlin97 0:7e6b349182bc 156 }
jvfausto 10:beaa2ddfef32 157
ryanlin97 0:7e6b349182bc 158 wait(process);
ryanlin97 0:7e6b349182bc 159 }
ryanlin97 0:7e6b349182bc 160 }