Jesus Fausto / Mbed OS wheelchaircontrolrealtime1

with pid foward right left foward

Dependencies:   wheelchaircontrolRosCom

Fork of wheelchaircontrolrealtime by ryan lin

main.cpp@8:db780b392bae, 2018-08-31 (annotated)

Committer:
jvfausto
Date:
Fri Aug 31 20:01:55 2018 +0000
Revision:
8:db780b392bae
Parent:
7:04f93e6b929f
Child:
9:1081ebfe3db0
j

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:7e6b349182bc 1 #include "wheelchair.h"
ryanlin97 0:7e6b349182bc 2
jvfausto 8:db780b392bae 3 QEI wheel(D0, D1, NC, 1200);
jvfausto 8:db780b392bae 4 DigitalIn pt3(D1, PullUp);
jvfausto 8:db780b392bae 5 DigitalIn pt4(D0, PullUp);
jvfausto 8:db780b392bae 6
jvfausto 8:db780b392bae 7 /*QEI wheel2 (D3, D6, NC, 1200);
jvfausto 8:db780b392bae 8 DigitalIn pt1(D3, PullUp);
jvfausto 8:db780b392bae 9 DigitalIn pt2(D6, PullUp);*/
jvfausto 8:db780b392bae 10
ryanlin97 0:7e6b349182bc 11 AnalogIn x(A0);
ryanlin97 0:7e6b349182bc 12 AnalogIn y(A1);
ryanlin97 0:7e6b349182bc 13
ryanlin97 0:7e6b349182bc 14 DigitalOut up(D2);
ryanlin97 0:7e6b349182bc 15 DigitalOut down(D3);
ryanlin97 6:e9b1684a9c00 16 DigitalOut on(D12);
ryanlin97 6:e9b1684a9c00 17 DigitalOut off(D11);
ryanlin97 0:7e6b349182bc 18 bool manual = false;
ryanlin97 0:7e6b349182bc 19
ryanlin97 0:7e6b349182bc 20 Serial pc(USBTX, USBRX, 57600);
ryanlin97 5:90bf5f0d86e9 21
ryanlin97 0:7e6b349182bc 22 Timer t;
ryanlin97 0:7e6b349182bc 23 EventQueue queue;
ryanlin97 0:7e6b349182bc 24
ryanlin97 0:7e6b349182bc 25 //MPU9250 imu(D14, D15);
jvfausto 8:db780b392bae 26 Wheelchair smart(xDir,yDir, &pc, &t, &wheel);
ryanlin97 0:7e6b349182bc 27 Thread thread;
ryanlin97 0:7e6b349182bc 28
ryanlin97 5:90bf5f0d86e9 29
ryanlin97 0:7e6b349182bc 30 int main(void)
ryanlin97 0:7e6b349182bc 31 {
ryanlin97 0:7e6b349182bc 32 queue.call_every(SAMPLEFREQ, &smart, &Wheelchair::compass_thread);
ryanlin97 0:7e6b349182bc 33 t.reset();
ryanlin97 0:7e6b349182bc 34 thread.start(callback(&queue, &EventQueue::dispatch_forever));
ryanlin97 0:7e6b349182bc 35 while(1) {
ryanlin97 0:7e6b349182bc 36 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 37 char c = pc.getc();
ryanlin97 0:7e6b349182bc 38 if( c == 'w') {
jvfausto 7:04f93e6b929f 39 //pc.printf("up \r\n");
ryanlin97 0:7e6b349182bc 40 smart.forward();
jvfausto 8:db780b392bae 41 pc.printf("%f\r\n", wheel.getDistance(53.975));
ryanlin97 0:7e6b349182bc 42 }
ryanlin97 0:7e6b349182bc 43 else if( c == 'a') {
jvfausto 7:04f93e6b929f 44 //pc.printf("left \r\n");
ryanlin97 0:7e6b349182bc 45 smart.left();
ryanlin97 0:7e6b349182bc 46 }
ryanlin97 0:7e6b349182bc 47
ryanlin97 0:7e6b349182bc 48 else if( c == 'd') {
jvfausto 7:04f93e6b929f 49 //pc.printf("right \r\n");
ryanlin97 0:7e6b349182bc 50 smart.right();
ryanlin97 0:7e6b349182bc 51 }
ryanlin97 0:7e6b349182bc 52
ryanlin97 0:7e6b349182bc 53 else if( c == 's') {
jvfausto 7:04f93e6b929f 54 //pc.printf("down \r\n");
ryanlin97 0:7e6b349182bc 55 smart.backward();
jvfausto 8:db780b392bae 56 // wheel.reset();
ryanlin97 0:7e6b349182bc 57 }
ryanlin97 0:7e6b349182bc 58
ryanlin97 0:7e6b349182bc 59 else if( c == 'r') {
ryanlin97 0:7e6b349182bc 60 smart.turn_right(90);
ryanlin97 0:7e6b349182bc 61 }
jvfausto 7:04f93e6b929f 62
ryanlin97 0:7e6b349182bc 63 else if( c == 'l') {
ryanlin97 0:7e6b349182bc 64 smart.turn_left(90);
ryanlin97 0:7e6b349182bc 65 }
ryanlin97 0:7e6b349182bc 66
ryanlin97 0:7e6b349182bc 67 else if( c == 't') {
jvfausto 8:db780b392bae 68
ryanlin97 0:7e6b349182bc 69 char buffer[256];
ryanlin97 0:7e6b349182bc 70 pc.printf ("Enter a long number: ");
jvfausto 8:db780b392bae 71 char d = pc.getc();
jvfausto 8:db780b392bae 72 int angle = 0;
jvfausto 7:04f93e6b929f 73 //fgets (buffer, 256, stdin);
jvfausto 8:db780b392bae 74 if(d == 'r')
jvfausto 8:db780b392bae 75 {
jvfausto 8:db780b392bae 76 angle = 90;//atoi (buffer);
jvfausto 8:db780b392bae 77 }
jvfausto 8:db780b392bae 78 if(d == 'l')
jvfausto 8:db780b392bae 79 {
jvfausto 8:db780b392bae 80 angle = -90;//atoi (buffer);
jvfausto 8:db780b392bae 81 }
ryanlin97 0:7e6b349182bc 82 if(angle == 0) {
jvfausto 7:04f93e6b929f 83 pc.printf("invalid input try again\r\n");
ryanlin97 0:7e6b349182bc 84 } else {
ryanlin97 0:7e6b349182bc 85 smart.pid_turn(angle);
ryanlin97 0:7e6b349182bc 86 }
ryanlin97 0:7e6b349182bc 87
ryanlin97 6:e9b1684a9c00 88 } else if(c == 'o') {
jvfausto 7:04f93e6b929f 89 pc.printf("turning on\r\n");
ryanlin97 6:e9b1684a9c00 90 on = 1;
ryanlin97 6:e9b1684a9c00 91 wait(1);
ryanlin97 6:e9b1684a9c00 92 on = 0;
ryanlin97 6:e9b1684a9c00 93 } else if(c == 'f') {
jvfausto 7:04f93e6b929f 94 pc.printf("turning off\r\n");
ryanlin97 6:e9b1684a9c00 95 off = 1;
ryanlin97 6:e9b1684a9c00 96 wait(1);
ryanlin97 6:e9b1684a9c00 97 off = 0;
jvfausto 8:db780b392bae 98 } else if(c == 'k'){
jvfausto 8:db780b392bae 99 pc.printf("kitchen\r\n");
jvfausto 8:db780b392bae 100 smart.kitchen();
jvfausto 8:db780b392bae 101 } else if(c == 'e'){
jvfausto 8:db780b392bae 102 pc.printf("desk\r\n");
jvfausto 8:db780b392bae 103 smart.desk();
jvfausto 8:db780b392bae 104 } else if(c == 'x'){
jvfausto 8:db780b392bae 105 pc.printf("desk to kitchen\r\n");
jvfausto 8:db780b392bae 106 smart.desk_to_kitchen();
jvfausto 7:04f93e6b929f 107 } else if(c == 'u') {
jvfausto 8:db780b392bae 108 pc.printf ("Enter a long number: ");
jvfausto 8:db780b392bae 109 char d = pc.getc();
jvfausto 8:db780b392bae 110 double displacement = 0;
jvfausto 8:db780b392bae 111 if(d == '1')
jvfausto 8:db780b392bae 112 displacement = 5461;
jvfausto 8:db780b392bae 113 if(d == '2')
jvfausto 8:db780b392bae 114 displacement = 3658;
jvfausto 8:db780b392bae 115 if(d == '3')
jvfausto 8:db780b392bae 116 displacement = 305;
jvfausto 8:db780b392bae 117 if(d == '4')
jvfausto 8:db780b392bae 118 displacement = 1000;
jvfausto 7:04f93e6b929f 119 if(displacement > 0)
jvfausto 7:04f93e6b929f 120 {
jvfausto 8:db780b392bae 121 smart.pid_forward(displacement);
jvfausto 7:04f93e6b929f 122 }
jvfausto 7:04f93e6b929f 123 else if(displacement < 0)
jvfausto 7:04f93e6b929f 124 {
jvfausto 7:04f93e6b929f 125 smart.pid_reverse(displacement);
jvfausto 7:04f93e6b929f 126 }
ryanlin97 6:e9b1684a9c00 127 } else if( c == 'm' || manual) {
jvfausto 7:04f93e6b929f 128 pc.printf("turning on joystick\r\n");
ryanlin97 0:7e6b349182bc 129 manual = true;
ryanlin97 0:7e6b349182bc 130 t.reset();
ryanlin97 0:7e6b349182bc 131 while(manual) {
jvfausto 8:db780b392bae 132 smart.move(x,y);
ryanlin97 0:7e6b349182bc 133 if( pc.readable()) {
ryanlin97 0:7e6b349182bc 134 char d = pc.getc();
ryanlin97 0:7e6b349182bc 135 if( d == 'm') {
jvfausto 7:04f93e6b929f 136 pc.printf("turning off joystick\r\n");
ryanlin97 0:7e6b349182bc 137 manual = false;
ryanlin97 0:7e6b349182bc 138 }
ryanlin97 0:7e6b349182bc 139 }
jvfausto 7:04f93e6b929f 140 }
ryanlin97 0:7e6b349182bc 141 }
ryanlin97 0:7e6b349182bc 142 else {
jvfausto 7:04f93e6b929f 143 pc.printf("none \r\n");
ryanlin97 0:7e6b349182bc 144 smart.stop();
ryanlin97 0:7e6b349182bc 145 }
ryanlin97 0:7e6b349182bc 146 }
ryanlin97 0:7e6b349182bc 147
ryanlin97 0:7e6b349182bc 148 else {
jvfausto 7:04f93e6b929f 149 // pc.printf("Nothing pressed \r\n");
ryanlin97 0:7e6b349182bc 150 smart.stop();
ryanlin97 0:7e6b349182bc 151 }
ryanlin97 0:7e6b349182bc 152 wait(process);
ryanlin97 0:7e6b349182bc 153 }
ryanlin97 0:7e6b349182bc 154
ryanlin97 0:7e6b349182bc 155 }
ryanlin97 0:7e6b349182bc 156
ryanlin97 5:90bf5f0d86e9 157
ryanlin97 5:90bf5f0d86e9 158
ryanlin97 5:90bf5f0d86e9 159
ryanlin97 5:90bf5f0d86e9 160