ryan lin / Mbed 2 deprecated wheeldriveassistance

wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

main.cpp@10:e5463c11e0a0, 2018-07-23 (annotated)

Committer:
ryanlin97
Date:
Mon Jul 23 20:17:37 2018 +0000
Revision:
10:e5463c11e0a0
Parent:
8:381a4ec3fef8
Child:
11:75f0f13ff6c1
working with turns

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 0:fc0c4a184482 2
ryanlin97 3:a5e71bfdb492 3 AnalogIn x(A0);
ryanlin97 3:a5e71bfdb492 4 AnalogIn y(A1);
ryanlin97 3:a5e71bfdb492 5
ryanlin97 0:fc0c4a184482 6 DigitalOut off(D0);
ryanlin97 0:fc0c4a184482 7 DigitalOut on(D1);
ryanlin97 0:fc0c4a184482 8 DigitalOut up(D2);
ryanlin97 0:fc0c4a184482 9 DigitalOut down(D3);
ryanlin97 2:c2d049efc266 10
ryanlin97 10:e5463c11e0a0 11 bool manual = false;
ryanlin97 10:e5463c11e0a0 12
ryanlin97 7:5e38d43fbce3 13 Serial pc(USBTX, USBRX, 57600);
ryanlin97 7:5e38d43fbce3 14 Timer t;
ryanlin97 6:0cd57bdd8fbc 15
ryanlin97 10:e5463c11e0a0 16 MPU9250 imu(D14, D15);
ryanlin97 8:381a4ec3fef8 17 //Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 2:c2d049efc266 18
ryanlin97 0:fc0c4a184482 19 int main(void)
ryanlin97 0:fc0c4a184482 20 {
ryanlin97 10:e5463c11e0a0 21
ryanlin97 10:e5463c11e0a0 22 uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ryanlin97 10:e5463c11e0a0 23 pc.printf("I AM 0x%x\n\r", whoami);
ryanlin97 10:e5463c11e0a0 24 pc.printf("I SHOULD BE 0x71\n\r");
ryanlin97 10:e5463c11e0a0 25
ryanlin97 10:e5463c11e0a0 26 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
ryanlin97 10:e5463c11e0a0 27 pc.printf("MPU9250 is online...\n\r");
ryanlin97 10:e5463c11e0a0 28
ryanlin97 10:e5463c11e0a0 29 wait(1);
ryanlin97 10:e5463c11e0a0 30
ryanlin97 10:e5463c11e0a0 31 imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
ryanlin97 10:e5463c11e0a0 32 imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ryanlin97 10:e5463c11e0a0 33 imu.initMPU9250();
ryanlin97 10:e5463c11e0a0 34 imu.initAK8963(imu.magCalibration);
ryanlin97 10:e5463c11e0a0 35 wait(2);
ryanlin97 10:e5463c11e0a0 36
ryanlin97 10:e5463c11e0a0 37 } else {
ryanlin97 10:e5463c11e0a0 38 pc.printf("Could not connect to MPU9250: \n\r");
ryanlin97 10:e5463c11e0a0 39 pc.printf("%#x \n", whoami);
ryanlin97 10:e5463c11e0a0 40
ryanlin97 10:e5463c11e0a0 41 while(1) ; // Loop forever if communication doesn't happen
ryanlin97 7:5e38d43fbce3 42 }
ryanlin97 7:5e38d43fbce3 43
ryanlin97 7:5e38d43fbce3 44 imu.getAres(); // Get accelerometer sensitivity
ryanlin97 7:5e38d43fbce3 45 imu.getGres(); // Get gyro sensitivity
ryanlin97 7:5e38d43fbce3 46 imu.getMres(); // Get magnetometer sensitivity
ryanlin97 7:5e38d43fbce3 47 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
ryanlin97 7:5e38d43fbce3 48 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
ryanlin97 7:5e38d43fbce3 49 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
ryanlin97 7:5e38d43fbce3 50 imu.magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ryanlin97 7:5e38d43fbce3 51 imu.magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ryanlin97 7:5e38d43fbce3 52 imu.magbias[2] = +125.; // User environmental x-axis correction in milliGauss
ryanlin97 10:e5463c11e0a0 53
ryanlin97 10:e5463c11e0a0 54
ryanlin97 8:381a4ec3fef8 55 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 10:e5463c11e0a0 56
ryanlin97 6:0cd57bdd8fbc 57 while(1) {
ryanlin97 10:e5463c11e0a0 58 if( pc.readable()) {
ryanlin97 10:e5463c11e0a0 59 char c = pc.getc();
ryanlin97 10:e5463c11e0a0 60
ryanlin97 10:e5463c11e0a0 61 if( c == 'w') {
ryanlin97 10:e5463c11e0a0 62 pc.printf("up \n");
ryanlin97 10:e5463c11e0a0 63 smart.forward();
ryanlin97 10:e5463c11e0a0 64 }
ryanlin97 6:0cd57bdd8fbc 65
ryanlin97 10:e5463c11e0a0 66 else if( c == 'd') {
ryanlin97 10:e5463c11e0a0 67 pc.printf("left \n");
ryanlin97 10:e5463c11e0a0 68 smart.left();
ryanlin97 10:e5463c11e0a0 69 }
ryanlin97 6:0cd57bdd8fbc 70
ryanlin97 10:e5463c11e0a0 71 else if( c == 'a') {
ryanlin97 10:e5463c11e0a0 72 pc.printf("right \n");
ryanlin97 10:e5463c11e0a0 73 smart.right();
ryanlin97 10:e5463c11e0a0 74 }
ryanlin97 6:0cd57bdd8fbc 75
ryanlin97 10:e5463c11e0a0 76 else if( c == 's') {
ryanlin97 10:e5463c11e0a0 77 pc.printf("down \n");
ryanlin97 10:e5463c11e0a0 78 smart.backward();
ryanlin97 10:e5463c11e0a0 79 }
ryanlin97 6:0cd57bdd8fbc 80
ryanlin97 10:e5463c11e0a0 81 else if( c == 'r') {
ryanlin97 10:e5463c11e0a0 82 smart.turn_right();
ryanlin97 10:e5463c11e0a0 83 }
ryanlin97 6:0cd57bdd8fbc 84
ryanlin97 10:e5463c11e0a0 85 else if( c == 'l') {
ryanlin97 10:e5463c11e0a0 86 smart.turn_left();
ryanlin97 10:e5463c11e0a0 87 }
ryanlin97 6:0cd57bdd8fbc 88
ryanlin97 10:e5463c11e0a0 89 else if( c == 'm' || manual) {
ryanlin97 10:e5463c11e0a0 90 pc.printf("turning on joystick\n");
ryanlin97 10:e5463c11e0a0 91 manual = true;
ryanlin97 10:e5463c11e0a0 92 while(manual) {
ryanlin97 10:e5463c11e0a0 93 smart.move(x,y);
ryanlin97 10:e5463c11e0a0 94 if( pc.readable()) {
ryanlin97 10:e5463c11e0a0 95 char d = pc.getc();
ryanlin97 10:e5463c11e0a0 96 if( d == 'm') {
ryanlin97 10:e5463c11e0a0 97 pc.printf("turning off joystick\n");
ryanlin97 10:e5463c11e0a0 98 manual = false;
ryanlin97 10:e5463c11e0a0 99 }
ryanlin97 10:e5463c11e0a0 100 }
ryanlin97 10:e5463c11e0a0 101 }
ryanlin97 10:e5463c11e0a0 102 }
ryanlin97 6:0cd57bdd8fbc 103
ryanlin97 10:e5463c11e0a0 104 else {
ryanlin97 10:e5463c11e0a0 105 pc.printf("none \n");
ryanlin97 10:e5463c11e0a0 106 smart.stop();
ryanlin97 10:e5463c11e0a0 107 }
ryanlin97 10:e5463c11e0a0 108 }
ryanlin97 6:0cd57bdd8fbc 109
ryanlin97 10:e5463c11e0a0 110 else {
ryanlin97 10:e5463c11e0a0 111 // pc.printf("Nothing pressed \n");
ryanlin97 10:e5463c11e0a0 112 smart.stop();
ryanlin97 10:e5463c11e0a0 113 }
ryanlin97 10:e5463c11e0a0 114 /*
ryanlin97 6:0cd57bdd8fbc 115 smart.move(x,y);
ryanlin97 6:0cd57bdd8fbc 116 if( pc.readable()) {
ryanlin97 6:0cd57bdd8fbc 117 char c = pc.getc();
ryanlin97 6:0cd57bdd8fbc 118 if( c == 'r') {
ryanlin97 7:5e38d43fbce3 119 smart.turn_right();
ryanlin97 0:fc0c4a184482 120 }
ryanlin97 8:381a4ec3fef8 121 if( c == 'l') {
ryanlin97 8:381a4ec3fef8 122 smart.turn_left();
ryanlin97 10:e5463c11e0a0 123 }
ryanlin97 10:e5463c11e0a0 124 }*/
ryanlin97 6:0cd57bdd8fbc 125 wait(process);
ryanlin97 6:0cd57bdd8fbc 126 }
ryanlin97 1:c0beadca1617 127
ryanlin97 0:fc0c4a184482 128 }
ryanlin97 0:fc0c4a184482 129