with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

Committer:
ryanlin97
Date:
Thu Aug 09 16:37:08 2018 +0000
Revision:
0:7e6b349182bc
Child:
3:ef063fd4234e
Child:
5:90bf5f0d86e9
incorporated encoder and pid. pid needs fixing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
ryanlin97 0:7e6b349182bc 2
ryanlin97 0:7e6b349182bc 3 AnalogIn x(A0);
ryanlin97 0:7e6b349182bc 4 AnalogIn y(A1);
ryanlin97 0:7e6b349182bc 5
ryanlin97 0:7e6b349182bc 6 DigitalOut off(D0);
ryanlin97 0:7e6b349182bc 7 DigitalOut on(D1);
ryanlin97 0:7e6b349182bc 8 DigitalOut up(D2);
ryanlin97 0:7e6b349182bc 9 DigitalOut down(D3);
ryanlin97 0:7e6b349182bc 10
ryanlin97 0:7e6b349182bc 11 bool manual = false;
ryanlin97 0:7e6b349182bc 12
ryanlin97 0:7e6b349182bc 13 Serial pc(USBTX, USBRX, 57600);
ryanlin97 0:7e6b349182bc 14 Timer t;
ryanlin97 0:7e6b349182bc 15 EventQueue queue;
ryanlin97 0:7e6b349182bc 16
ryanlin97 0:7e6b349182bc 17 //MPU9250 imu(D14, D15);
ryanlin97 0:7e6b349182bc 18 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 0:7e6b349182bc 19 Thread thread;
ryanlin97 0:7e6b349182bc 20
ryanlin97 0:7e6b349182bc 21 int main(void)
ryanlin97 0:7e6b349182bc 22 {
ryanlin97 0:7e6b349182bc 23 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
ryanlin97 0:7e6b349182bc 24 t.reset();
ryanlin97 0:7e6b349182bc 25 thread.start(callback(&queue, &EventQueue::dispatch_forever));
ryanlin97 0:7e6b349182bc 26 while(1) {
ryanlin97 0:7e6b349182bc 27 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 28 char c = pc.getc();
ryanlin97 0:7e6b349182bc 29
ryanlin97 0:7e6b349182bc 30 if( c == 'w') {
ryanlin97 0:7e6b349182bc 31 pc.printf("up \n");
ryanlin97 0:7e6b349182bc 32 smart.forward();
ryanlin97 0:7e6b349182bc 33 }
ryanlin97 0:7e6b349182bc 34
ryanlin97 0:7e6b349182bc 35 else if( c == 'a') {
ryanlin97 0:7e6b349182bc 36 //pc.printf("left \n");
ryanlin97 0:7e6b349182bc 37 smart.left();
ryanlin97 0:7e6b349182bc 38 }
ryanlin97 0:7e6b349182bc 39
ryanlin97 0:7e6b349182bc 40 else if( c == 'd') {
ryanlin97 0:7e6b349182bc 41 //pc.printf("right \n");
ryanlin97 0:7e6b349182bc 42 smart.right();
ryanlin97 0:7e6b349182bc 43 }
ryanlin97 0:7e6b349182bc 44
ryanlin97 0:7e6b349182bc 45 else if( c == 's') {
ryanlin97 0:7e6b349182bc 46 pc.printf("down \n");
ryanlin97 0:7e6b349182bc 47 smart.backward();
ryanlin97 0:7e6b349182bc 48 }
ryanlin97 0:7e6b349182bc 49
ryanlin97 0:7e6b349182bc 50 else if( c == 'r') {
ryanlin97 0:7e6b349182bc 51 smart.turn_right(90);
ryanlin97 0:7e6b349182bc 52 }
ryanlin97 0:7e6b349182bc 53
ryanlin97 0:7e6b349182bc 54 else if( c == 'l') {
ryanlin97 0:7e6b349182bc 55 smart.turn_left(90);
ryanlin97 0:7e6b349182bc 56 }
ryanlin97 0:7e6b349182bc 57
ryanlin97 0:7e6b349182bc 58 else if( c == 't') {
ryanlin97 0:7e6b349182bc 59 char buffer[256];
ryanlin97 0:7e6b349182bc 60 pc.printf ("Enter a long number: ");
ryanlin97 0:7e6b349182bc 61 fgets (buffer, 256, stdin);
ryanlin97 0:7e6b349182bc 62 int angle = atoi (buffer);
ryanlin97 0:7e6b349182bc 63
ryanlin97 0:7e6b349182bc 64 if(angle == 0) {
ryanlin97 0:7e6b349182bc 65 pc.printf("invalid input try again\n");
ryanlin97 0:7e6b349182bc 66 } else {
ryanlin97 0:7e6b349182bc 67 smart.pid_turn(angle);
ryanlin97 0:7e6b349182bc 68 }
ryanlin97 0:7e6b349182bc 69
ryanlin97 0:7e6b349182bc 70 }
ryanlin97 0:7e6b349182bc 71
ryanlin97 0:7e6b349182bc 72 else if( c == 'm' || manual) {
ryanlin97 0:7e6b349182bc 73 pc.printf("turning on joystick\n");
ryanlin97 0:7e6b349182bc 74 manual = true;
ryanlin97 0:7e6b349182bc 75 t.reset();
ryanlin97 0:7e6b349182bc 76 while(manual) {
ryanlin97 0:7e6b349182bc 77 smart.move(x,y);
ryanlin97 0:7e6b349182bc 78 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 79 char d = pc.getc();
ryanlin97 0:7e6b349182bc 80 if( d == 'm') {
ryanlin97 0:7e6b349182bc 81 pc.printf("turning off joystick\n");
ryanlin97 0:7e6b349182bc 82 manual = false;
ryanlin97 0:7e6b349182bc 83 }
ryanlin97 0:7e6b349182bc 84 }
ryanlin97 0:7e6b349182bc 85 }
ryanlin97 0:7e6b349182bc 86 }
ryanlin97 0:7e6b349182bc 87
ryanlin97 0:7e6b349182bc 88 else {
ryanlin97 0:7e6b349182bc 89 pc.printf("none \n");
ryanlin97 0:7e6b349182bc 90 smart.stop();
ryanlin97 0:7e6b349182bc 91 }
ryanlin97 0:7e6b349182bc 92 }
ryanlin97 0:7e6b349182bc 93
ryanlin97 0:7e6b349182bc 94 else {
ryanlin97 0:7e6b349182bc 95 // pc.printf("Nothing pressed \n");
ryanlin97 0:7e6b349182bc 96 smart.stop();
ryanlin97 0:7e6b349182bc 97 }
ryanlin97 0:7e6b349182bc 98 wait(process);
ryanlin97 0:7e6b349182bc 99 }
ryanlin97 0:7e6b349182bc 100
ryanlin97 0:7e6b349182bc 101 }
ryanlin97 0:7e6b349182bc 102