ryan lin / Mbed 2 deprecated wheeldriveassistance

wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

main.cpp@7:5e38d43fbce3, 2018-07-22 (annotated)

Committer:
ryanlin97
Date:
Sun Jul 22 06:15:03 2018 +0000
Revision:
7:5e38d43fbce3
Parent:
6:0cd57bdd8fbc
Child:
8:381a4ec3fef8
turning

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 7:5e38d43fbce3 11 Serial pc(USBTX, USBRX, 57600);
ryanlin97 7:5e38d43fbce3 12 Timer t;
ryanlin97 7:5e38d43fbce3 13 Timer other;
ryanlin97 6:0cd57bdd8fbc 14
ryanlin97 7:5e38d43fbce3 15 MPU9250 imu(D14, D15);
ryanlin97 7:5e38d43fbce3 16 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 2:c2d049efc266 17
ryanlin97 0:fc0c4a184482 18 int main(void)
ryanlin97 0:fc0c4a184482 19 {
ryanlin97 7:5e38d43fbce3 20
ryanlin97 7:5e38d43fbce3 21 uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ryanlin97 7:5e38d43fbce3 22 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
ryanlin97 7:5e38d43fbce3 23
ryanlin97 7:5e38d43fbce3 24 if (whoami == 0x71) // WHO_AM_I should always be 0x68
ryanlin97 7:5e38d43fbce3 25 {
ryanlin97 7:5e38d43fbce3 26 pc.printf("MPU9250 is online...\n\r");
ryanlin97 7:5e38d43fbce3 27
ryanlin97 7:5e38d43fbce3 28 wait(1);
ryanlin97 7:5e38d43fbce3 29
ryanlin97 7:5e38d43fbce3 30 imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
ryanlin97 7:5e38d43fbce3 31 imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ryanlin97 7:5e38d43fbce3 32 imu.initMPU9250();
ryanlin97 7:5e38d43fbce3 33 imu.initAK8963(imu.magCalibration);
ryanlin97 7:5e38d43fbce3 34 wait(2);
ryanlin97 7:5e38d43fbce3 35 }
ryanlin97 7:5e38d43fbce3 36 else
ryanlin97 7:5e38d43fbce3 37 {
ryanlin97 7:5e38d43fbce3 38 pc.printf("Could not connect to MPU9250: \n\r");
ryanlin97 7:5e38d43fbce3 39 pc.printf("%#x \n", whoami);
ryanlin97 7:5e38d43fbce3 40
ryanlin97 7:5e38d43fbce3 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 7:5e38d43fbce3 53
ryanlin97 7:5e38d43fbce3 54
ryanlin97 0:fc0c4a184482 55 on = 1;
ryanlin97 6:0cd57bdd8fbc 56 while(1) {
ryanlin97 7:5e38d43fbce3 57 /* if( pc.readable()) {
ryanlin97 6:0cd57bdd8fbc 58 char c = pc.getc();
ryanlin97 6:0cd57bdd8fbc 59 if( c == 'w') {
ryanlin97 6:0cd57bdd8fbc 60 pc.printf("up \n");
ryanlin97 6:0cd57bdd8fbc 61 smart.forward();
ryanlin97 6:0cd57bdd8fbc 62 }
ryanlin97 6:0cd57bdd8fbc 63
ryanlin97 6:0cd57bdd8fbc 64 else if( c == 'd') {
ryanlin97 6:0cd57bdd8fbc 65 pc.printf("left \n");
ryanlin97 6:0cd57bdd8fbc 66 smart.left();
ryanlin97 6:0cd57bdd8fbc 67 }
ryanlin97 6:0cd57bdd8fbc 68
ryanlin97 6:0cd57bdd8fbc 69 else if( c == 'a') {
ryanlin97 6:0cd57bdd8fbc 70 pc.printf("right \n");
ryanlin97 6:0cd57bdd8fbc 71 smart.right();
ryanlin97 6:0cd57bdd8fbc 72 }
ryanlin97 6:0cd57bdd8fbc 73
ryanlin97 6:0cd57bdd8fbc 74 else if( c == 's') {
ryanlin97 6:0cd57bdd8fbc 75 pc.printf("down \n");
ryanlin97 6:0cd57bdd8fbc 76 smart.backward();
ryanlin97 6:0cd57bdd8fbc 77 }
ryanlin97 6:0cd57bdd8fbc 78
ryanlin97 6:0cd57bdd8fbc 79 else {
ryanlin97 6:0cd57bdd8fbc 80 pc.printf("none \n");
ryanlin97 6:0cd57bdd8fbc 81 smart.stop();
ryanlin97 6:0cd57bdd8fbc 82 if( c == 'o') {
ryanlin97 6:0cd57bdd8fbc 83 pc.printf("turning on");
ryanlin97 6:0cd57bdd8fbc 84 on = 0;
ryanlin97 6:0cd57bdd8fbc 85 wait(process);
ryanlin97 6:0cd57bdd8fbc 86 on = 1;
ryanlin97 6:0cd57bdd8fbc 87 }
ryanlin97 6:0cd57bdd8fbc 88
ryanlin97 6:0cd57bdd8fbc 89 else if( c == 'k') {
ryanlin97 6:0cd57bdd8fbc 90 off = 0;
ryanlin97 6:0cd57bdd8fbc 91 wait(process);
ryanlin97 6:0cd57bdd8fbc 92 off = 1;
ryanlin97 6:0cd57bdd8fbc 93 }
ryanlin97 6:0cd57bdd8fbc 94
ryanlin97 6:0cd57bdd8fbc 95 else if( c == 'u') {
ryanlin97 6:0cd57bdd8fbc 96 up = 0;
ryanlin97 6:0cd57bdd8fbc 97 wait(process);
ryanlin97 6:0cd57bdd8fbc 98 up = 1;
ryanlin97 6:0cd57bdd8fbc 99 }
ryanlin97 6:0cd57bdd8fbc 100
ryanlin97 6:0cd57bdd8fbc 101 else if( c == 'p') {
ryanlin97 6:0cd57bdd8fbc 102 down = 0;
ryanlin97 6:0cd57bdd8fbc 103 wait(process);
ryanlin97 6:0cd57bdd8fbc 104 down = 1;
ryanlin97 6:0cd57bdd8fbc 105 }
ryanlin97 6:0cd57bdd8fbc 106 }
ryanlin97 6:0cd57bdd8fbc 107 }
ryanlin97 6:0cd57bdd8fbc 108
ryanlin97 6:0cd57bdd8fbc 109 else {
ryanlin97 6:0cd57bdd8fbc 110 pc.printf("Nothing pressed \n");
ryanlin97 6:0cd57bdd8fbc 111 smart.stop();
ryanlin97 6:0cd57bdd8fbc 112 }
ryanlin97 7:5e38d43fbce3 113 */
ryanlin97 6:0cd57bdd8fbc 114 smart.move(x,y);
ryanlin97 6:0cd57bdd8fbc 115 if( pc.readable()) {
ryanlin97 6:0cd57bdd8fbc 116 char c = pc.getc();
ryanlin97 6:0cd57bdd8fbc 117 if( c == 'r') {
ryanlin97 7:5e38d43fbce3 118 smart.turn_right();
ryanlin97 0:fc0c4a184482 119 }
ryanlin97 6:0cd57bdd8fbc 120 }
ryanlin97 6:0cd57bdd8fbc 121 wait(process);
ryanlin97 6:0cd57bdd8fbc 122 }
ryanlin97 1:c0beadca1617 123
ryanlin97 0:fc0c4a184482 124 }
ryanlin97 0:fc0c4a184482 125