with pid foward right left foward
Dependencies: wheelchaircontrolRosCom
Fork of wheelchaircontrolrealtime by
Diff: main.cpp
- Revision:
- 9:1081ebfe3db0
- Parent:
- 8:db780b392bae
- Child:
- 10:beaa2ddfef32
diff -r db780b392bae -r 1081ebfe3db0 main.cpp --- a/main.cpp Fri Aug 31 20:01:55 2018 +0000 +++ b/main.cpp Fri Nov 02 02:28:51 2018 +0000 @@ -1,160 +1,135 @@ #include "wheelchair.h" -QEI wheel(D0, D1, NC, 1200); -DigitalIn pt3(D1, PullUp); +QEI wheel(D0, D1, NC, 1200); //Initializes right encoder +DigitalIn pt3(D1, PullUp); //Pull up resistors to read analog signals into digital signals DigitalIn pt4(D0, PullUp); -/*QEI wheel2 (D3, D6, NC, 1200); -DigitalIn pt1(D3, PullUp); -DigitalIn pt2(D6, PullUp);*/ +QEI wheelS (D3, D6, NC, 1200); //Initializes Left encoder +DigitalIn pt1(D3, PullUp); //Pull up resistors to read analog signals into digital signals +DigitalIn pt2(D6, PullUp); -AnalogIn x(A0); +AnalogIn x(A0); //Initializes analog axis for the joystick AnalogIn y(A1); -DigitalOut up(D2); -DigitalOut down(D3); -DigitalOut on(D12); -DigitalOut off(D11); -bool manual = false; +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 +bool manual = false; //Turns chair joystic to automatic and viceverza -Serial pc(USBTX, USBRX, 57600); +Serial pc(USBTX, USBRX, 57600); //Serial Monitor + +Timer t; //Initialize time object t +EventQueue queue; //Class to organize threads -Timer t; -EventQueue queue; +ros::NodeHandle nh; +nav_msgs::Odometry odom; +ros::Publisher chatter("odom", &odom); -//MPU9250 imu(D14, D15); -Wheelchair smart(xDir,yDir, &pc, &t, &wheel); -Thread thread; +/*void setOdomNode(){ + nh.initNode(); + nh.advertise(chatter); +}*/ +Wheelchair smart(xDir,yDir, &pc, &t, &wheel, &wheelS); //Initialize wheelchair object +Thread compass; //Thread for compass +Thread velosity; //Thread for velosity +Thread ros_com; //Thread for velosity int main(void) -{ - queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); +{ + nh.initNode(); + nh.advertise(chatter); + + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread); //Sets up sampling frequency of the compass_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::velosity_thread); //Sets up sampling frequency of the velosity_thread + queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::rosCom_thread); //Sets up sampling frequency of the velosity_thread t.reset(); - thread.start(callback(&queue, &EventQueue::dispatch_forever)); + compass.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the compass thread + velosity.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread + ros_com.start(callback(&queue, &EventQueue::dispatch_forever)); //Starts running the velosity thread while(1) { if( pc.readable()) { - char c = pc.getc(); + char c = pc.getc(); //Read the instruction sent if( c == 'w') { - //pc.printf("up \r\n"); - smart.forward(); - pc.printf("%f\r\n", wheel.getDistance(53.975)); + smart.forward(); //Move foward } else if( c == 'a') { - //pc.printf("left \r\n"); - smart.left(); + smart.left(); //Turn left } - else if( c == 'd') { - //pc.printf("right \r\n"); - smart.right(); + smart.right(); //Turn right } - else if( c == 's') { - //pc.printf("down \r\n"); - smart.backward(); - // wheel.reset(); + smart.backward(); //Turn rackwards } - - else if( c == 'r') { - smart.turn_right(90); - } - - else if( c == 'l') { - smart.turn_left(90); - } - - else if( c == 't') { - - char buffer[256]; - pc.printf ("Enter a long number: "); - char d = pc.getc(); + + else if( c == 't') { + smart.pid_twistA(); + /* + char buffer[256]; //Buffer for Angle to turn + pc.printf ("Enter a long number: "); int angle = 0; - //fgets (buffer, 256, stdin); + + //if you can not send a string + char d = pc.getc(); //Reads wether to turn right or left if(d == 'r') { - angle = 90;//atoi (buffer); + angle = 90; } if(d == 'l') { - angle = -90;//atoi (buffer); + angle = -90; } + + //if you can send a string + /* + fgets (buffer, 256, stdin); //Reads string and puts it in a buffer + angle = atoi(buffer); //Converst string into integer + */ + /* if(angle == 0) { - pc.printf("invalid input try again\r\n"); + pc.printf("invalid input try again\r\n"); } else { - smart.pid_turn(angle); - } - - } else if(c == 'o') { + smart.pid_turn(angle); //Sends instruction to turn desired angle + }*/ + } 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') { + } else if(c == 'f') { //Turns off chair pc.printf("turning off\r\n"); off = 1; wait(1); off = 0; - } else if(c == 'k'){ - pc.printf("kitchen\r\n"); - smart.kitchen(); - } else if(c == 'e'){ - pc.printf("desk\r\n"); - smart.desk(); - } else if(c == 'x'){ - pc.printf("desk to kitchen\r\n"); - smart.desk_to_kitchen(); - } else if(c == 'u') { - pc.printf ("Enter a long number: "); - char d = pc.getc(); - double displacement = 0; - if(d == '1') - displacement = 5461; - if(d == '2') - displacement = 3658; - if(d == '3') - displacement = 305; - if(d == '4') - displacement = 1000; - if(displacement > 0) - { - smart.pid_forward(displacement); - } - else if(displacement < 0) - { - smart.pid_reverse(displacement); - } - } else if( c == 'm' || manual) { + + } 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); + smart.move(x,y); //Reads from joystick and moves if( pc.readable()) { char d = pc.getc(); - if( d == 'm') { + 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(); + } else { + pc.printf("none \r\n"); + smart.stop(); //If nothing else is happening stop the chair } } - else { - // pc.printf("Nothing pressed \r\n"); - smart.stop(); + smart.stop(); //If nothing else is happening stop the chair } wait(process); } - } - - - - -