Jesus Fausto / Mbed OS wheelchaircontrolrealtime1

with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@9:1081ebfe3db0, 2018-11-02 (annotated)

Committer:
jvfausto
Date:
Fri Nov 02 02:28:51 2018 +0000
Revision:
9:1081ebfe3db0
Parent:
8:db780b392bae
Child:
10:beaa2ddfef32
1

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 9:1081ebfe3db0 27 ros::Publisher chatter("odom", &odom);
ryanlin97 0:7e6b349182bc 28
jvfausto 9:1081ebfe3db0 29 /*void setOdomNode(){
jvfausto 9:1081ebfe3db0 30 nh.initNode();
jvfausto 9:1081ebfe3db0 31 nh.advertise(chatter);
jvfausto 9:1081ebfe3db0 32 }*/
ryanlin97 0:7e6b349182bc 33
jvfausto 9:1081ebfe3db0 34 Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS); //Initialize wheelchair object
jvfausto 9:1081ebfe3db0 35 Thread compass; //Thread for compass
jvfausto 9:1081ebfe3db0 36 Thread velosity; //Thread for velosity
jvfausto 9:1081ebfe3db0 37 Thread ros_com; //Thread for velosity
ryanlin97 5:90bf5f0d86e9 38
ryanlin97 0:7e6b349182bc 39 int main(void)
jvfausto 9:1081ebfe3db0 40 {
jvfausto 9:1081ebfe3db0 41 nh.initNode();
jvfausto 9:1081ebfe3db0 42 nh.advertise(chatter);
jvfausto 9:1081ebfe3db0 43
jvfausto 9:1081ebfe3db0 44 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread
jvfausto 9:1081ebfe3db0 45 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velosity_thread); //Sets up sampling frequency of the velosity_thread
jvfausto 9:1081ebfe3db0 46 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::rosCom_thread); //Sets up sampling frequency of the velosity_thread
ryanlin97 0:7e6b349182bc 47 t.reset();
jvfausto 9:1081ebfe3db0 48 compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread
jvfausto 9:1081ebfe3db0 49 velosity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
jvfausto 9:1081ebfe3db0 50 ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread
ryanlin97 0:7e6b349182bc 51 while(1) {
ryanlin97 0:7e6b349182bc 52 if( pc.readable()) {
jvfausto 9:1081ebfe3db0 53 char c = pc.getc(); //Read the instruction sent
ryanlin97 0:7e6b349182bc 54 if( c == 'w') {
jvfausto 9:1081ebfe3db0 55 smart.forward(); //Move foward
ryanlin97 0:7e6b349182bc 56 }
ryanlin97 0:7e6b349182bc 57 else if( c == 'a') {
jvfausto 9:1081ebfe3db0 58 smart.left(); //Turn left
ryanlin97 0:7e6b349182bc 59 }
ryanlin97 0:7e6b349182bc 60 else if( c == 'd') {
jvfausto 9:1081ebfe3db0 61 smart.right(); //Turn right
ryanlin97 0:7e6b349182bc 62 }
ryanlin97 0:7e6b349182bc 63 else if( c == 's') {
jvfausto 9:1081ebfe3db0 64 smart.backward(); //Turn rackwards
ryanlin97 0:7e6b349182bc 65 }
jvfausto 9:1081ebfe3db0 66
jvfausto 9:1081ebfe3db0 67 else if( c == 't') {
jvfausto 9:1081ebfe3db0 68 smart.pid_twistA();
jvfausto 9:1081ebfe3db0 69 /*
jvfausto 9:1081ebfe3db0 70 char buffer[256]; //Buffer for Angle to turn
jvfausto 9:1081ebfe3db0 71 pc.printf ("Enter a long number: ");
jvfausto 8:db780b392bae 72 int angle = 0;
jvfausto 9:1081ebfe3db0 73
jvfausto 9:1081ebfe3db0 74 //if you can not send a string
jvfausto 9:1081ebfe3db0 75 char d = pc.getc(); //Reads wether to turn right or left
jvfausto 8:db780b392bae 76 if(d == 'r')
jvfausto 8:db780b392bae 77 {
jvfausto 9:1081ebfe3db0 78 angle = 90;
jvfausto 8:db780b392bae 79 }
jvfausto 8:db780b392bae 80 if(d == 'l')
jvfausto 8:db780b392bae 81 {
jvfausto 9:1081ebfe3db0 82 angle = -90;
jvfausto 8:db780b392bae 83 }
jvfausto 9:1081ebfe3db0 84
jvfausto 9:1081ebfe3db0 85 //if you can send a string
jvfausto 9:1081ebfe3db0 86 /*
jvfausto 9:1081ebfe3db0 87 fgets (buffer, 256, stdin); //Reads string and puts it in a buffer
jvfausto 9:1081ebfe3db0 88 angle = atoi(buffer); //Converst string into integer
jvfausto 9:1081ebfe3db0 89 */
jvfausto 9:1081ebfe3db0 90 /*
ryanlin97 0:7e6b349182bc 91 if(angle == 0) {
jvfausto 9:1081ebfe3db0 92 pc.printf("invalid input try again\r\n");
ryanlin97 0:7e6b349182bc 93 } else {
jvfausto 9:1081ebfe3db0 94 smart.pid_turn(angle); //Sends instruction to turn desired angle
jvfausto 9:1081ebfe3db0 95 }*/
jvfausto 9:1081ebfe3db0 96 } else if(c == 'v'){
jvfausto 9:1081ebfe3db0 97 smart.showOdom();
jvfausto 9:1081ebfe3db0 98 } else if(c == 'o') { //Turns on chair
jvfausto 7:04f93e6b929f 99 pc.printf("turning on\r\n");
ryanlin97 6:e9b1684a9c00 100 on = 1;
ryanlin97 6:e9b1684a9c00 101 wait(1);
ryanlin97 6:e9b1684a9c00 102 on = 0;
jvfausto 9:1081ebfe3db0 103 } else if(c == 'f') { //Turns off chair
jvfausto 7:04f93e6b929f 104 pc.printf("turning off\r\n");
ryanlin97 6:e9b1684a9c00 105 off = 1;
ryanlin97 6:e9b1684a9c00 106 wait(1);
ryanlin97 6:e9b1684a9c00 107 off = 0;
jvfausto 9:1081ebfe3db0 108
jvfausto 9:1081ebfe3db0 109 } else if(c == 'k'){ //Sends command to go to the kitchen
jvfausto 9:1081ebfe3db0 110 smart.pid_twistV();
jvfausto 9:1081ebfe3db0 111 } else if( c == 'm' || manual) { //Turns wheelchair to joystick
jvfausto 7:04f93e6b929f 112 pc.printf("turning on joystick\r\n");
ryanlin97 0:7e6b349182bc 113 manual = true;
ryanlin97 0:7e6b349182bc 114 t.reset();
ryanlin97 0:7e6b349182bc 115 while(manual) {
jvfausto 9:1081ebfe3db0 116 smart.move(x,y); //Reads from joystick and moves
ryanlin97 0:7e6b349182bc 117 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 118 char d = pc.getc();
jvfausto 9:1081ebfe3db0 119 if( d == 'm') { //Turns wheelchair from joystick into auto
jvfausto 7:04f93e6b929f 120 pc.printf("turning off joystick\r\n");
ryanlin97 0:7e6b349182bc 121 manual = false;
ryanlin97 0:7e6b349182bc 122 }
ryanlin97 0:7e6b349182bc 123 }
jvfausto 7:04f93e6b929f 124 }
jvfausto 9:1081ebfe3db0 125 } else {
jvfausto 9:1081ebfe3db0 126 pc.printf("none \r\n");
jvfausto 9:1081ebfe3db0 127 smart.stop(); //If nothing else is happening stop the chair
ryanlin97 0:7e6b349182bc 128 }
ryanlin97 0:7e6b349182bc 129 }
ryanlin97 0:7e6b349182bc 130 else {
jvfausto 9:1081ebfe3db0 131 smart.stop(); //If nothing else is happening stop the chair
ryanlin97 0:7e6b349182bc 132 }
ryanlin97 0:7e6b349182bc 133 wait(process);
ryanlin97 0:7e6b349182bc 134 }
ryanlin97 0:7e6b349182bc 135 }