Jesus Fausto / Mbed OS wheelchaircontrolrealtime1

with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@7:04f93e6b929f, 2018-08-29 (annotated)

Committer:
jvfausto
Date:
Wed Aug 29 16:53:41 2018 +0000
Revision:
7:04f93e6b929f
Parent:
6:e9b1684a9c00
Child:
8:db780b392bae
with pid foward right left foward

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 up(D2);
ryanlin97 0:7e6b349182bc 7 DigitalOut down(D3);
ryanlin97 6:e9b1684a9c00 8 DigitalOut on(D12);
ryanlin97 6:e9b1684a9c00 9 DigitalOut off(D11);
ryanlin97 0:7e6b349182bc 10 bool manual = false;
ryanlin97 0:7e6b349182bc 11
ryanlin97 0:7e6b349182bc 12 Serial pc(USBTX, USBRX, 57600);
ryanlin97 5:90bf5f0d86e9 13
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;
jvfausto 7:04f93e6b929f 20 Thread thread2;
ryanlin97 0:7e6b349182bc 21
ryanlin97 5:90bf5f0d86e9 22
ryanlin97 0:7e6b349182bc 23 int main(void)
ryanlin97 0:7e6b349182bc 24 {
ryanlin97 0:7e6b349182bc 25 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
jvfausto 7:04f93e6b929f 26 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::distance_thread);
ryanlin97 0:7e6b349182bc 27 t.reset();
ryanlin97 0:7e6b349182bc 28 thread.start(callback(&queue, &EventQueue::dispatch_forever));
jvfausto 7:04f93e6b929f 29 thread2.start(callback(&queue, &EventQueue::dispatch_forever));
jvfausto 7:04f93e6b929f 30 // thread2.start(callback(&queue, &EventQueue::dispatch_forever));
ryanlin97 0:7e6b349182bc 31 while(1) {
ryanlin97 0:7e6b349182bc 32 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 33 char c = pc.getc();
ryanlin97 0:7e6b349182bc 34 if( c == 'w') {
jvfausto 7:04f93e6b929f 35 //pc.printf("up \r\n");
ryanlin97 0:7e6b349182bc 36 smart.forward();
ryanlin97 0:7e6b349182bc 37 }
ryanlin97 0:7e6b349182bc 38
ryanlin97 0:7e6b349182bc 39 else if( c == 'a') {
jvfausto 7:04f93e6b929f 40 //pc.printf("left \r\n");
ryanlin97 0:7e6b349182bc 41 smart.left();
ryanlin97 0:7e6b349182bc 42 }
ryanlin97 0:7e6b349182bc 43
ryanlin97 0:7e6b349182bc 44 else if( c == 'd') {
jvfausto 7:04f93e6b929f 45 //pc.printf("right \r\n");
ryanlin97 0:7e6b349182bc 46 smart.right();
ryanlin97 0:7e6b349182bc 47 }
ryanlin97 0:7e6b349182bc 48
ryanlin97 0:7e6b349182bc 49 else if( c == 's') {
jvfausto 7:04f93e6b929f 50 //pc.printf("down \r\n");
ryanlin97 0:7e6b349182bc 51 smart.backward();
ryanlin97 0:7e6b349182bc 52 }
ryanlin97 0:7e6b349182bc 53
ryanlin97 0:7e6b349182bc 54 else if( c == 'r') {
ryanlin97 0:7e6b349182bc 55 smart.turn_right(90);
ryanlin97 0:7e6b349182bc 56 }
jvfausto 7:04f93e6b929f 57
ryanlin97 0:7e6b349182bc 58 else if( c == 'l') {
ryanlin97 0:7e6b349182bc 59 smart.turn_left(90);
ryanlin97 0:7e6b349182bc 60 }
ryanlin97 0:7e6b349182bc 61
ryanlin97 0:7e6b349182bc 62 else if( c == 't') {
ryanlin97 0:7e6b349182bc 63 char buffer[256];
ryanlin97 0:7e6b349182bc 64 pc.printf ("Enter a long number: ");
jvfausto 7:04f93e6b929f 65 //fgets (buffer, 256, stdin);
jvfausto 7:04f93e6b929f 66 int angle = 90;//atoi (buffer);
ryanlin97 0:7e6b349182bc 67
ryanlin97 0:7e6b349182bc 68 if(angle == 0) {
jvfausto 7:04f93e6b929f 69 pc.printf("invalid input try again\r\n");
ryanlin97 0:7e6b349182bc 70 } else {
ryanlin97 0:7e6b349182bc 71 smart.pid_turn(angle);
ryanlin97 0:7e6b349182bc 72 }
ryanlin97 0:7e6b349182bc 73
ryanlin97 6:e9b1684a9c00 74 } else if(c == 'o') {
jvfausto 7:04f93e6b929f 75 pc.printf("turning on\r\n");
ryanlin97 6:e9b1684a9c00 76 on = 1;
ryanlin97 6:e9b1684a9c00 77 wait(1);
ryanlin97 6:e9b1684a9c00 78 on = 0;
ryanlin97 6:e9b1684a9c00 79 } else if(c == 'f') {
jvfausto 7:04f93e6b929f 80 pc.printf("turning off\r\n");
ryanlin97 6:e9b1684a9c00 81 off = 1;
ryanlin97 6:e9b1684a9c00 82 wait(1);
ryanlin97 6:e9b1684a9c00 83 off = 0;
jvfausto 7:04f93e6b929f 84 } else if(c == 'u') {
jvfausto 7:04f93e6b929f 85 double displacement = -1000;
jvfausto 7:04f93e6b929f 86 if(displacement > 0)
jvfausto 7:04f93e6b929f 87 {
jvfausto 7:04f93e6b929f 88 smart.pid_foward(displacement);
jvfausto 7:04f93e6b929f 89 }
jvfausto 7:04f93e6b929f 90 else if(displacement < 0)
jvfausto 7:04f93e6b929f 91 {
jvfausto 7:04f93e6b929f 92 smart.pid_reverse(displacement);
jvfausto 7:04f93e6b929f 93 }
ryanlin97 6:e9b1684a9c00 94 } else if( c == 'm' || manual) {
jvfausto 7:04f93e6b929f 95 pc.printf("turning on joystick\r\n");
ryanlin97 0:7e6b349182bc 96 manual = true;
ryanlin97 0:7e6b349182bc 97 t.reset();
ryanlin97 0:7e6b349182bc 98 while(manual) {
ryanlin97 0:7e6b349182bc 99 smart.move(x,y);
ryanlin97 0:7e6b349182bc 100 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 101 char d = pc.getc();
ryanlin97 0:7e6b349182bc 102 if( d == 'm') {
jvfausto 7:04f93e6b929f 103 pc.printf("turning off joystick\r\n");
ryanlin97 0:7e6b349182bc 104 manual = false;
ryanlin97 0:7e6b349182bc 105 }
ryanlin97 0:7e6b349182bc 106 }
jvfausto 7:04f93e6b929f 107 }
ryanlin97 0:7e6b349182bc 108 }
ryanlin97 0:7e6b349182bc 109 else {
jvfausto 7:04f93e6b929f 110 pc.printf("none \r\n");
ryanlin97 0:7e6b349182bc 111 smart.stop();
ryanlin97 0:7e6b349182bc 112 }
ryanlin97 0:7e6b349182bc 113 }
ryanlin97 0:7e6b349182bc 114
ryanlin97 0:7e6b349182bc 115 else {
jvfausto 7:04f93e6b929f 116 // pc.printf("Nothing pressed \r\n");
ryanlin97 0:7e6b349182bc 117 smart.stop();
ryanlin97 0:7e6b349182bc 118 }
ryanlin97 0:7e6b349182bc 119 wait(process);
ryanlin97 0:7e6b349182bc 120 }
ryanlin97 0:7e6b349182bc 121
ryanlin97 0:7e6b349182bc 122 }
ryanlin97 0:7e6b349182bc 123
ryanlin97 5:90bf5f0d86e9 124
ryanlin97 5:90bf5f0d86e9 125
ryanlin97 5:90bf5f0d86e9 126
ryanlin97 5:90bf5f0d86e9 127