wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

Committer:
ryanlin97
Date:
Sun Jul 22 18:46:24 2018 +0000
Revision:
8:381a4ec3fef8
Parent:
7:5e38d43fbce3
Child:
10:e5463c11e0a0
updated for mpu;

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 8:381a4ec3fef8 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 8:381a4ec3fef8 55 Wheelchair smart(xDir,yDir, &pc, &t);
ryanlin97 8:381a4ec3fef8 56
ryanlin97 6:0cd57bdd8fbc 57 while(1) {
ryanlin97 7:5e38d43fbce3 58 /* if( pc.readable()) {
ryanlin97 6:0cd57bdd8fbc 59 char c = pc.getc();
ryanlin97 6:0cd57bdd8fbc 60 if( c == 'w') {
ryanlin97 6:0cd57bdd8fbc 61 pc.printf("up \n");
ryanlin97 6:0cd57bdd8fbc 62 smart.forward();
ryanlin97 6:0cd57bdd8fbc 63 }
ryanlin97 6:0cd57bdd8fbc 64
ryanlin97 6:0cd57bdd8fbc 65 else if( c == 'd') {
ryanlin97 6:0cd57bdd8fbc 66 pc.printf("left \n");
ryanlin97 6:0cd57bdd8fbc 67 smart.left();
ryanlin97 6:0cd57bdd8fbc 68 }
ryanlin97 6:0cd57bdd8fbc 69
ryanlin97 6:0cd57bdd8fbc 70 else if( c == 'a') {
ryanlin97 6:0cd57bdd8fbc 71 pc.printf("right \n");
ryanlin97 6:0cd57bdd8fbc 72 smart.right();
ryanlin97 6:0cd57bdd8fbc 73 }
ryanlin97 6:0cd57bdd8fbc 74
ryanlin97 6:0cd57bdd8fbc 75 else if( c == 's') {
ryanlin97 6:0cd57bdd8fbc 76 pc.printf("down \n");
ryanlin97 6:0cd57bdd8fbc 77 smart.backward();
ryanlin97 6:0cd57bdd8fbc 78 }
ryanlin97 6:0cd57bdd8fbc 79
ryanlin97 6:0cd57bdd8fbc 80 else {
ryanlin97 6:0cd57bdd8fbc 81 pc.printf("none \n");
ryanlin97 6:0cd57bdd8fbc 82 smart.stop();
ryanlin97 6:0cd57bdd8fbc 83 if( c == 'o') {
ryanlin97 6:0cd57bdd8fbc 84 pc.printf("turning on");
ryanlin97 6:0cd57bdd8fbc 85 on = 0;
ryanlin97 6:0cd57bdd8fbc 86 wait(process);
ryanlin97 6:0cd57bdd8fbc 87 on = 1;
ryanlin97 6:0cd57bdd8fbc 88 }
ryanlin97 6:0cd57bdd8fbc 89
ryanlin97 6:0cd57bdd8fbc 90 else if( c == 'k') {
ryanlin97 6:0cd57bdd8fbc 91 off = 0;
ryanlin97 6:0cd57bdd8fbc 92 wait(process);
ryanlin97 6:0cd57bdd8fbc 93 off = 1;
ryanlin97 6:0cd57bdd8fbc 94 }
ryanlin97 6:0cd57bdd8fbc 95
ryanlin97 6:0cd57bdd8fbc 96 else if( c == 'u') {
ryanlin97 6:0cd57bdd8fbc 97 up = 0;
ryanlin97 6:0cd57bdd8fbc 98 wait(process);
ryanlin97 6:0cd57bdd8fbc 99 up = 1;
ryanlin97 6:0cd57bdd8fbc 100 }
ryanlin97 6:0cd57bdd8fbc 101
ryanlin97 6:0cd57bdd8fbc 102 else if( c == 'p') {
ryanlin97 6:0cd57bdd8fbc 103 down = 0;
ryanlin97 6:0cd57bdd8fbc 104 wait(process);
ryanlin97 6:0cd57bdd8fbc 105 down = 1;
ryanlin97 6:0cd57bdd8fbc 106 }
ryanlin97 6:0cd57bdd8fbc 107 }
ryanlin97 6:0cd57bdd8fbc 108 }
ryanlin97 6:0cd57bdd8fbc 109
ryanlin97 6:0cd57bdd8fbc 110 else {
ryanlin97 6:0cd57bdd8fbc 111 pc.printf("Nothing pressed \n");
ryanlin97 6:0cd57bdd8fbc 112 smart.stop();
ryanlin97 6:0cd57bdd8fbc 113 }
ryanlin97 7:5e38d43fbce3 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 8:381a4ec3fef8 123 }
ryanlin97 6:0cd57bdd8fbc 124 }
ryanlin97 6:0cd57bdd8fbc 125 wait(process);
ryanlin97 6:0cd57bdd8fbc 126 }
ryanlin97 1:c0beadca1617 127
ryanlin97 0:fc0c4a184482 128 }
ryanlin97 0:fc0c4a184482 129