FRA221_2015 / Mbed 2 deprecated Taebox

B16

Dependencies:   mbed

Code.cpp@0:b0ed3674f5b2, 2015-12-09 (annotated)

Committer:
57340500084
Date:
Wed Dec 09 02:50:22 2015 +0000
Revision:
0:b0ed3674f5b2
16B

Who changed what in which revision?

UserRevisionLine numberNew contents of line
57340500084 0:b0ed3674f5b2 1 /*****
57340500084 0:b0ed3674f5b2 2 Algorithm based on MPU-9250_Snowda program. It has been modified by Josu? Olmeda Castell? for imasD Tecnolog?a.
57340500084 0:b0ed3674f5b2 3
57340500084 0:b0ed3674f5b2 4 This algorithm calibrates and reads data from accelerometer, gyroscope, magnetometer and the
57340500084 0:b0ed3674f5b2 5 internal temperature sensor. The lecture is made each time has a new mesured value (both gyro and accel data).
57340500084 0:b0ed3674f5b2 6 A comunication with a computer is made using serial interface. The user can see the data measured with 1 second update rate.
57340500084 0:b0ed3674f5b2 7
57340500084 0:b0ed3674f5b2 8 This algorithm uses the STM32L152 development board and the MPU-9250 9-axis InvenSense movement sensor. The communication
57340500084 0:b0ed3674f5b2 9 between both devices is made through I2C serial interface.
57340500084 0:b0ed3674f5b2 10
57340500084 0:b0ed3674f5b2 11 AD0 should be connected to GND.
57340500084 0:b0ed3674f5b2 12
57340500084 0:b0ed3674f5b2 13 04/05/2015
57340500084 0:b0ed3674f5b2 14 *****/
57340500084 0:b0ed3674f5b2 15
57340500084 0:b0ed3674f5b2 16 #include "mbed.h"
57340500084 0:b0ed3674f5b2 17 #include "mpu9250.h"
57340500084 0:b0ed3674f5b2 18 #include "DHT.h"
57340500084 0:b0ed3674f5b2 19
57340500084 0:b0ed3674f5b2 20 Serial pc(D8,D2); // Huyperterminal default config: 9600 bauds, 8-bit data, 1 stop bit, no parity
57340500084 0:b0ed3674f5b2 21 Serial bell(PA_11,PA_12);
57340500084 0:b0ed3674f5b2 22 MPU9250 mpu9250;
57340500084 0:b0ed3674f5b2 23 Timer t;
57340500084 0:b0ed3674f5b2 24 Timer t2;
57340500084 0:b0ed3674f5b2 25 DigitalOut myled1(LED1);
57340500084 0:b0ed3674f5b2 26 DigitalOut myledred(D10);
57340500084 0:b0ed3674f5b2 27 DigitalOut myledblue(D9);
57340500084 0:b0ed3674f5b2 28 DigitalOut myledbuzzer(D7);
57340500084 0:b0ed3674f5b2 29 DHT sensor(D4,22); // Use the SEN11301P sensor
57340500084 0:b0ed3674f5b2 30
57340500084 0:b0ed3674f5b2 31 void StopCon(float,float,float);
57340500084 0:b0ed3674f5b2 32 void WalkCon(float);
57340500084 0:b0ed3674f5b2 33 void RunCon(float);
57340500084 0:b0ed3674f5b2 34 void checkTemp(float);
57340500084 0:b0ed3674f5b2 35 void timer2(float,float,float);
57340500084 0:b0ed3674f5b2 36
57340500084 0:b0ed3674f5b2 37 float sum = 0;
57340500084 0:b0ed3674f5b2 38 uint32_t sumCount = 0;
57340500084 0:b0ed3674f5b2 39 char buffer[14];
57340500084 0:b0ed3674f5b2 40 uint8_t dato_leido[2];
57340500084 0:b0ed3674f5b2 41 uint8_t whoami;
57340500084 0:b0ed3674f5b2 42 char showtimez[10];
57340500084 0:b0ed3674f5b2 43 int timez=0;
57340500084 0:b0ed3674f5b2 44 int err;
57340500084 0:b0ed3674f5b2 45 int cont = 0;
57340500084 0:b0ed3674f5b2 46 int sw = 1;
57340500084 0:b0ed3674f5b2 47 int check = 0;
57340500084 0:b0ed3674f5b2 48 int a=0;
57340500084 0:b0ed3674f5b2 49 float y1 = 0;
57340500084 0:b0ed3674f5b2 50 float y2 = 0;
57340500084 0:b0ed3674f5b2 51 float newyaw = 0;
57340500084 0:b0ed3674f5b2 52
57340500084 0:b0ed3674f5b2 53 uint8_t state_menu = 0;
57340500084 0:b0ed3674f5b2 54 uint8_t state_show = 0;
57340500084 0:b0ed3674f5b2 55 uint8_t state_exit = 0;
57340500084 0:b0ed3674f5b2 56 uint8_t data;
57340500084 0:b0ed3674f5b2 57
57340500084 0:b0ed3674f5b2 58 int main()
57340500084 0:b0ed3674f5b2 59 {
57340500084 0:b0ed3674f5b2 60
57340500084 0:b0ed3674f5b2 61 //___ Set up I2C: use fast (400 kHz) I2C ___
57340500084 0:b0ed3674f5b2 62 i2c.frequency(400000);
57340500084 0:b0ed3674f5b2 63
57340500084 0:b0ed3674f5b2 64 pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
57340500084 0:b0ed3674f5b2 65
57340500084 0:b0ed3674f5b2 66 t.start(); // Timer ON
57340500084 0:b0ed3674f5b2 67
57340500084 0:b0ed3674f5b2 68 // Read the WHO_AM_I register, this is a good test of communication
57340500084 0:b0ed3674f5b2 69 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
57340500084 0:b0ed3674f5b2 70
57340500084 0:b0ed3674f5b2 71 pc.printf("I AM 0x%x\n\r", whoami);
57340500084 0:b0ed3674f5b2 72 pc.printf("I SHOULD BE 0x71\n\r");
57340500084 0:b0ed3674f5b2 73 if (I2Cstate != 0) // error on I2C
57340500084 0:b0ed3674f5b2 74 pc.printf("I2C failure while reading WHO_AM_I register");
57340500084 0:b0ed3674f5b2 75
57340500084 0:b0ed3674f5b2 76 if (whoami == 0x71) { // WHO_AM_I should always be 0x71
57340500084 0:b0ed3674f5b2 77 pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
57340500084 0:b0ed3674f5b2 78 pc.printf("MPU9250 is online...\n\r");
57340500084 0:b0ed3674f5b2 79 sprintf(buffer, "0x%x", whoami);
57340500084 0:b0ed3674f5b2 80 wait(1);
57340500084 0:b0ed3674f5b2 81
57340500084 0:b0ed3674f5b2 82 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
57340500084 0:b0ed3674f5b2 83
57340500084 0:b0ed3674f5b2 84 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
57340500084 0:b0ed3674f5b2 85 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
57340500084 0:b0ed3674f5b2 86 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
57340500084 0:b0ed3674f5b2 87 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
57340500084 0:b0ed3674f5b2 88 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
57340500084 0:b0ed3674f5b2 89 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
57340500084 0:b0ed3674f5b2 90 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
57340500084 0:b0ed3674f5b2 91
57340500084 0:b0ed3674f5b2 92 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
57340500084 0:b0ed3674f5b2 93 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
57340500084 0:b0ed3674f5b2 94 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
57340500084 0:b0ed3674f5b2 95 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
57340500084 0:b0ed3674f5b2 96 pc.printf("x accel bias = %f\n\r", accelBias[0]);
57340500084 0:b0ed3674f5b2 97 pc.printf("y accel bias = %f\n\r", accelBias[1]);
57340500084 0:b0ed3674f5b2 98 pc.printf("z accel bias = %f\n\r", accelBias[2]);
57340500084 0:b0ed3674f5b2 99 wait(2);
57340500084 0:b0ed3674f5b2 100
57340500084 0:b0ed3674f5b2 101 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
57340500084 0:b0ed3674f5b2 102 mpu9250.initMPU9250();
57340500084 0:b0ed3674f5b2 103 //pc.printf("MPU9250 initialized for active data mode....\n\r");
57340500084 0:b0ed3674f5b2 104
57340500084 0:b0ed3674f5b2 105 // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
57340500084 0:b0ed3674f5b2 106 mpu9250.initAK8963(magCalibration);
57340500084 0:b0ed3674f5b2 107 pc.printf("AK8963 initialized for active data mode....\n\r");
57340500084 0:b0ed3674f5b2 108 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
57340500084 0:b0ed3674f5b2 109 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
57340500084 0:b0ed3674f5b2 110 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
57340500084 0:b0ed3674f5b2 111 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
57340500084 0:b0ed3674f5b2 112 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
57340500084 0:b0ed3674f5b2 113 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
57340500084 0:b0ed3674f5b2 114 wait(1);
57340500084 0:b0ed3674f5b2 115 }
57340500084 0:b0ed3674f5b2 116
57340500084 0:b0ed3674f5b2 117 else { // Connection failure
57340500084 0:b0ed3674f5b2 118 pc.printf("Could not connect to MPU9250: \n\r");
57340500084 0:b0ed3674f5b2 119 pc.printf("%#x \n", whoami);
57340500084 0:b0ed3674f5b2 120 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
57340500084 0:b0ed3674f5b2 121 while(1) ; // Loop forever if communication doesn't happen
57340500084 0:b0ed3674f5b2 122 }
57340500084 0:b0ed3674f5b2 123
57340500084 0:b0ed3674f5b2 124 mpu9250.getAres(); // Get accelerometer sensitivity
57340500084 0:b0ed3674f5b2 125 mpu9250.getGres(); // Get gyro sensitivity
57340500084 0:b0ed3674f5b2 126 mpu9250.getMres(); // Get magnetometer sensitivity
57340500084 0:b0ed3674f5b2 127 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
57340500084 0:b0ed3674f5b2 128 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
57340500084 0:b0ed3674f5b2 129 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
57340500084 0:b0ed3674f5b2 130 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
57340500084 0:b0ed3674f5b2 131 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
57340500084 0:b0ed3674f5b2 132 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
57340500084 0:b0ed3674f5b2 133
57340500084 0:b0ed3674f5b2 134 while(1) {
57340500084 0:b0ed3674f5b2 135
57340500084 0:b0ed3674f5b2 136 // If intPin goes high, all data registers have new data
57340500084 0:b0ed3674f5b2 137 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
57340500084 0:b0ed3674f5b2 138
57340500084 0:b0ed3674f5b2 139 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
57340500084 0:b0ed3674f5b2 140 // Now we'll calculate the accleration value into actual g's
57340500084 0:b0ed3674f5b2 141 if (I2Cstate != 0) //error on I2C
57340500084 0:b0ed3674f5b2 142 pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
57340500084 0:b0ed3674f5b2 143 else { // I2C read or write ok
57340500084 0:b0ed3674f5b2 144 I2Cstate = 1;
57340500084 0:b0ed3674f5b2 145 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
57340500084 0:b0ed3674f5b2 146 ay = (float)accelCount[1]*aRes - accelBias[1];
57340500084 0:b0ed3674f5b2 147 az = (float)accelCount[2]*aRes - accelBias[2];
57340500084 0:b0ed3674f5b2 148 }
57340500084 0:b0ed3674f5b2 149
57340500084 0:b0ed3674f5b2 150 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
57340500084 0:b0ed3674f5b2 151 // Calculate the gyro value into actual degrees per second
57340500084 0:b0ed3674f5b2 152 if (I2Cstate != 0) //error on I2C
57340500084 0:b0ed3674f5b2 153 pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
57340500084 0:b0ed3674f5b2 154 else { // I2C read or write ok
57340500084 0:b0ed3674f5b2 155 I2Cstate = 1;
57340500084 0:b0ed3674f5b2 156 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
57340500084 0:b0ed3674f5b2 157 gy = (float)gyroCount[1]*gRes - gyroBias[1];
57340500084 0:b0ed3674f5b2 158 gz = (float)gyroCount[2]*gRes - gyroBias[2];
57340500084 0:b0ed3674f5b2 159 }
57340500084 0:b0ed3674f5b2 160
57340500084 0:b0ed3674f5b2 161 mpu9250.readMagData(magCount); // Read the x/y/z adc values
57340500084 0:b0ed3674f5b2 162 // Calculate the magnetometer values in milliGauss
57340500084 0:b0ed3674f5b2 163 // Include factory calibration per data sheet and user environmental corrections
57340500084 0:b0ed3674f5b2 164 if (I2Cstate != 0) //error on I2C
57340500084 0:b0ed3674f5b2 165 pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
57340500084 0:b0ed3674f5b2 166 else { // I2C read or write ok
57340500084 0:b0ed3674f5b2 167 I2Cstate = 1;
57340500084 0:b0ed3674f5b2 168 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
57340500084 0:b0ed3674f5b2 169 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
57340500084 0:b0ed3674f5b2 170 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
57340500084 0:b0ed3674f5b2 171 }
57340500084 0:b0ed3674f5b2 172
57340500084 0:b0ed3674f5b2 173 mpu9250.getCompassOrientation(orientation);
57340500084 0:b0ed3674f5b2 174 }
57340500084 0:b0ed3674f5b2 175
57340500084 0:b0ed3674f5b2 176 Now = t.read_us();
57340500084 0:b0ed3674f5b2 177 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
57340500084 0:b0ed3674f5b2 178 lastUpdate = Now;
57340500084 0:b0ed3674f5b2 179 sum += deltat;
57340500084 0:b0ed3674f5b2 180 sumCount++;
57340500084 0:b0ed3674f5b2 181
57340500084 0:b0ed3674f5b2 182 // Pass gyro rate as rad/s
57340500084 0:b0ed3674f5b2 183 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
57340500084 0:b0ed3674f5b2 184 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
57340500084 0:b0ed3674f5b2 185
57340500084 0:b0ed3674f5b2 186
57340500084 0:b0ed3674f5b2 187 // Serial print and/or display at 1.5 s rate independent of data rates
57340500084 0:b0ed3674f5b2 188 delt_t = t.read_ms() - count;
57340500084 0:b0ed3674f5b2 189 if (delt_t > 100) { // update LCD once per half-second independent of read rate
57340500084 0:b0ed3674f5b2 190
57340500084 0:b0ed3674f5b2 191
57340500084 0:b0ed3674f5b2 192
57340500084 0:b0ed3674f5b2 193 tempCount = mpu9250.readTempData(); // Read the adc values
57340500084 0:b0ed3674f5b2 194 if (I2Cstate != 0) //error on I2C
57340500084 0:b0ed3674f5b2 195 pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
57340500084 0:b0ed3674f5b2 196 else { // I2C read or write ok
57340500084 0:b0ed3674f5b2 197 I2Cstate = 1;
57340500084 0:b0ed3674f5b2 198 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
57340500084 0:b0ed3674f5b2 199
57340500084 0:b0ed3674f5b2 200
57340500084 0:b0ed3674f5b2 201
57340500084 0:b0ed3674f5b2 202
57340500084 0:b0ed3674f5b2 203 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
57340500084 0:b0ed3674f5b2 204 // In this coordinate system, the positive z-axis is down toward Earth.
57340500084 0:b0ed3674f5b2 205 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
57340500084 0:b0ed3674f5b2 206 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
57340500084 0:b0ed3674f5b2 207 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
57340500084 0:b0ed3674f5b2 208 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
57340500084 0:b0ed3674f5b2 209 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
57340500084 0:b0ed3674f5b2 210 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
57340500084 0:b0ed3674f5b2 211 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
57340500084 0:b0ed3674f5b2 212
57340500084 0:b0ed3674f5b2 213 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
57340500084 0:b0ed3674f5b2 214 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
57340500084 0:b0ed3674f5b2 215 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
57340500084 0:b0ed3674f5b2 216 pitch *= 180.0f / PI;
57340500084 0:b0ed3674f5b2 217 yaw *= 180.0f / PI;
57340500084 0:b0ed3674f5b2 218 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
57340500084 0:b0ed3674f5b2 219 roll *= 180.0f / PI;
57340500084 0:b0ed3674f5b2 220
57340500084 0:b0ed3674f5b2 221 timer2(yaw,pitch,roll);
57340500084 0:b0ed3674f5b2 222 sensor.readData();
57340500084 0:b0ed3674f5b2 223 checkTemp(sensor.ReadTemperature(CELCIUS));
57340500084 0:b0ed3674f5b2 224
57340500084 0:b0ed3674f5b2 225 if(state_show == 0) {
57340500084 0:b0ed3674f5b2 226 pc.printf("\nWelcome to Project Digital B16\nplease select 1 or 2\n");
57340500084 0:b0ed3674f5b2 227 pc.printf("1. Mode a\n");
57340500084 0:b0ed3674f5b2 228 pc.printf("2. Mode b\n");
57340500084 0:b0ed3674f5b2 229 state_show = 1;
57340500084 0:b0ed3674f5b2 230 }
57340500084 0:b0ed3674f5b2 231 if(pc.readable()) {
57340500084 0:b0ed3674f5b2 232 data = pc.getc();
57340500084 0:b0ed3674f5b2 233 pc.printf("\n");
57340500084 0:b0ed3674f5b2 234 state_show = 0;
57340500084 0:b0ed3674f5b2 235 state_exit = 0;
57340500084 0:b0ed3674f5b2 236 }
57340500084 0:b0ed3674f5b2 237 switch(data) {
57340500084 0:b0ed3674f5b2 238 case '1':
57340500084 0:b0ed3674f5b2 239 do {
57340500084 0:b0ed3674f5b2 240 if(state_menu == 0) {
57340500084 0:b0ed3674f5b2 241 pc.printf("Temperature is %4.2f C \r\n",sensor.ReadTemperature(CELCIUS));
57340500084 0:b0ed3674f5b2 242 pc.printf("Temperature is %4.2f F \r\n",sensor.ReadTemperature(FARENHEIT));
57340500084 0:b0ed3674f5b2 243 pc.printf("Temperature is %4.2f K \r\n",sensor.ReadTemperature(KELVIN));
57340500084 0:b0ed3674f5b2 244 pc.printf("Humidity is %4.2f \r\n",sensor.ReadHumidity());
57340500084 0:b0ed3674f5b2 245 pc.printf("x.Exit\n");
57340500084 0:b0ed3674f5b2 246 state_menu = 1;
57340500084 0:b0ed3674f5b2 247 }
57340500084 0:b0ed3674f5b2 248 if(pc.readable()) {
57340500084 0:b0ed3674f5b2 249 data = pc.getc();
57340500084 0:b0ed3674f5b2 250 pc.printf("\n");
57340500084 0:b0ed3674f5b2 251 state_menu=0;
57340500084 0:b0ed3674f5b2 252
57340500084 0:b0ed3674f5b2 253 switch(data) {
57340500084 0:b0ed3674f5b2 254 case'x':
57340500084 0:b0ed3674f5b2 255 state_exit = 1;
57340500084 0:b0ed3674f5b2 256 break;
57340500084 0:b0ed3674f5b2 257
57340500084 0:b0ed3674f5b2 258 default:
57340500084 0:b0ed3674f5b2 259 pc.printf("plz select x for exit\n");
57340500084 0:b0ed3674f5b2 260 pc.printf("\n\n");
57340500084 0:b0ed3674f5b2 261 break;
57340500084 0:b0ed3674f5b2 262 }
57340500084 0:b0ed3674f5b2 263 }
57340500084 0:b0ed3674f5b2 264 } while(state_exit ==0);
57340500084 0:b0ed3674f5b2 265 pc.printf("\n\n");
57340500084 0:b0ed3674f5b2 266 break;
57340500084 0:b0ed3674f5b2 267
57340500084 0:b0ed3674f5b2 268 case'2':
57340500084 0:b0ed3674f5b2 269 do {
57340500084 0:b0ed3674f5b2 270 if(state_menu == 0) {
57340500084 0:b0ed3674f5b2 271 pc.printf("Check Status\n");
57340500084 0:b0ed3674f5b2 272 StopCon(yaw,pitch,roll);
57340500084 0:b0ed3674f5b2 273 WalkCon(az);
57340500084 0:b0ed3674f5b2 274 RunCon(roll);
57340500084 0:b0ed3674f5b2 275 pc.printf("x.Exit\n");
57340500084 0:b0ed3674f5b2 276 state_menu = 1;
57340500084 0:b0ed3674f5b2 277 }
57340500084 0:b0ed3674f5b2 278 if(pc.readable()) {
57340500084 0:b0ed3674f5b2 279 data = pc.getc();
57340500084 0:b0ed3674f5b2 280 pc.printf("\n");
57340500084 0:b0ed3674f5b2 281 state_menu=0;
57340500084 0:b0ed3674f5b2 282
57340500084 0:b0ed3674f5b2 283 switch(data) {
57340500084 0:b0ed3674f5b2 284 case'x':
57340500084 0:b0ed3674f5b2 285 state_exit = 1;
57340500084 0:b0ed3674f5b2 286 break;
57340500084 0:b0ed3674f5b2 287
57340500084 0:b0ed3674f5b2 288 default:
57340500084 0:b0ed3674f5b2 289 pc.printf("plz select x for exit\n");
57340500084 0:b0ed3674f5b2 290 pc.printf("\n\n");
57340500084 0:b0ed3674f5b2 291 break;
57340500084 0:b0ed3674f5b2 292 }
57340500084 0:b0ed3674f5b2 293 }
57340500084 0:b0ed3674f5b2 294 } while(state_exit ==0);
57340500084 0:b0ed3674f5b2 295 pc.printf("\n\n");
57340500084 0:b0ed3674f5b2 296 break;
57340500084 0:b0ed3674f5b2 297 }
57340500084 0:b0ed3674f5b2 298
57340500084 0:b0ed3674f5b2 299 //pc.printf("Ax , Ay , Az : %f %f %f\n\r", 10*ax , 10*ay , 10*az);
57340500084 0:b0ed3674f5b2 300 pc.printf(" %f %f %f\n\r", yaw, pitch, roll);
57340500084 0:b0ed3674f5b2 301
57340500084 0:b0ed3674f5b2 302 }
57340500084 0:b0ed3674f5b2 303 }
57340500084 0:b0ed3674f5b2 304 }
57340500084 0:b0ed3674f5b2 305 }
57340500084 0:b0ed3674f5b2 306
57340500084 0:b0ed3674f5b2 307 void StopCon(float Y,float P,float R)
57340500084 0:b0ed3674f5b2 308 {
57340500084 0:b0ed3674f5b2 309 while((Y == Y)&&(P >= -3.0f) && (P <= 3.0f) && (R >= -3.0f) && (R <= 3.0f)) {
57340500084 0:b0ed3674f5b2 310 cont++;
57340500084 0:b0ed3674f5b2 311 wait(1);
57340500084 0:b0ed3674f5b2 312 if(cont == 10) {
57340500084 0:b0ed3674f5b2 313 pc.printf("Stopping\n");
57340500084 0:b0ed3674f5b2 314 cont = 0;
57340500084 0:b0ed3674f5b2 315 /*myledblue = 1;
57340500084 0:b0ed3674f5b2 316 wait(3);
57340500084 0:b0ed3674f5b2 317 myledblue = 0;*/
57340500084 0:b0ed3674f5b2 318 break;
57340500084 0:b0ed3674f5b2 319 }
57340500084 0:b0ed3674f5b2 320 }
57340500084 0:b0ed3674f5b2 321
57340500084 0:b0ed3674f5b2 322 }
57340500084 0:b0ed3674f5b2 323 void WalkCon(float Z)
57340500084 0:b0ed3674f5b2 324 {
57340500084 0:b0ed3674f5b2 325 Z = Z*10;
57340500084 0:b0ed3674f5b2 326 while(Z >= 11.0f) {
57340500084 0:b0ed3674f5b2 327 cont++;
57340500084 0:b0ed3674f5b2 328 wait(1);
57340500084 0:b0ed3674f5b2 329 if(cont == 10) {
57340500084 0:b0ed3674f5b2 330 pc.printf("Walking\n");
57340500084 0:b0ed3674f5b2 331 /*myledred = 1;
57340500084 0:b0ed3674f5b2 332 wait(3);
57340500084 0:b0ed3674f5b2 333 myledred = 0;*/
57340500084 0:b0ed3674f5b2 334 cont = 0;
57340500084 0:b0ed3674f5b2 335 break;
57340500084 0:b0ed3674f5b2 336 }
57340500084 0:b0ed3674f5b2 337 }
57340500084 0:b0ed3674f5b2 338 }
57340500084 0:b0ed3674f5b2 339 void RunCon(float R)
57340500084 0:b0ed3674f5b2 340 {
57340500084 0:b0ed3674f5b2 341 while(R >= 10) {
57340500084 0:b0ed3674f5b2 342 cont++;
57340500084 0:b0ed3674f5b2 343 wait(1);
57340500084 0:b0ed3674f5b2 344 if(cont == 10) {
57340500084 0:b0ed3674f5b2 345 pc.printf("Running\n");
57340500084 0:b0ed3674f5b2 346 cont = 0;
57340500084 0:b0ed3674f5b2 347 /*myledblue = 1;
57340500084 0:b0ed3674f5b2 348 myledred = 1;
57340500084 0:b0ed3674f5b2 349 wait(0.25);
57340500084 0:b0ed3674f5b2 350 myledblue = 0;
57340500084 0:b0ed3674f5b2 351 myledred = 0;
57340500084 0:b0ed3674f5b2 352 myledblue = 1;
57340500084 0:b0ed3674f5b2 353 myledred = 1;
57340500084 0:b0ed3674f5b2 354 wait(0.25);
57340500084 0:b0ed3674f5b2 355 myledblue = 0;
57340500084 0:b0ed3674f5b2 356 myledred = 0;
57340500084 0:b0ed3674f5b2 357 wait(0.25);
57340500084 0:b0ed3674f5b2 358 myledblue = 1;
57340500084 0:b0ed3674f5b2 359 myledred = 1;
57340500084 0:b0ed3674f5b2 360 wait(0.25);
57340500084 0:b0ed3674f5b2 361 myledblue = 0;
57340500084 0:b0ed3674f5b2 362 myledred = 0;
57340500084 0:b0ed3674f5b2 363 myledblue = 1;
57340500084 0:b0ed3674f5b2 364 myledred = 1;
57340500084 0:b0ed3674f5b2 365 wait(0.25);
57340500084 0:b0ed3674f5b2 366 myledblue = 0;
57340500084 0:b0ed3674f5b2 367 myledred = 0;
57340500084 0:b0ed3674f5b2 368 myledblue = 1;
57340500084 0:b0ed3674f5b2 369 myledred = 1;
57340500084 0:b0ed3674f5b2 370 wait(0.25);
57340500084 0:b0ed3674f5b2 371 myledblue = 0;
57340500084 0:b0ed3674f5b2 372 myledred = 0;
57340500084 0:b0ed3674f5b2 373 wait(0.25);
57340500084 0:b0ed3674f5b2 374 myledblue = 1;
57340500084 0:b0ed3674f5b2 375 myledred = 1;
57340500084 0:b0ed3674f5b2 376 wait(0.25);
57340500084 0:b0ed3674f5b2 377 myledblue = 0;
57340500084 0:b0ed3674f5b2 378 myledred = 0;*/
57340500084 0:b0ed3674f5b2 379 break;
57340500084 0:b0ed3674f5b2 380 }
57340500084 0:b0ed3674f5b2 381 }
57340500084 0:b0ed3674f5b2 382 }
57340500084 0:b0ed3674f5b2 383 void checkTemp(float T)
57340500084 0:b0ed3674f5b2 384 {
57340500084 0:b0ed3674f5b2 385 if(T >= 38 && T <= 39) {
57340500084 0:b0ed3674f5b2 386 myledblue = 1;
57340500084 0:b0ed3674f5b2 387 } else myledblue = 0;
57340500084 0:b0ed3674f5b2 388 if(T >= 40) {
57340500084 0:b0ed3674f5b2 389 myledred = 1;
57340500084 0:b0ed3674f5b2 390 } else myledred = 0;
57340500084 0:b0ed3674f5b2 391 }
57340500084 0:b0ed3674f5b2 392 void timer2(float y,float p,float r)
57340500084 0:b0ed3674f5b2 393 {
57340500084 0:b0ed3674f5b2 394 y2 = y;
57340500084 0:b0ed3674f5b2 395 newyaw = y1 - y2;
57340500084 0:b0ed3674f5b2 396 y1 = y2;
57340500084 0:b0ed3674f5b2 397
57340500084 0:b0ed3674f5b2 398 if((newyaw <= -10 || newyaw >= 10) && (p <= -13 || p >= 13)&& (r <= -15 || r >= 5 ) && sw == 1) {
57340500084 0:b0ed3674f5b2 399 pc.printf(" *****444444******* \n ");
57340500084 0:b0ed3674f5b2 400 check=1;
57340500084 0:b0ed3674f5b2 401 t2.start();
57340500084 0:b0ed3674f5b2 402 sw=0;
57340500084 0:b0ed3674f5b2 403 }
57340500084 0:b0ed3674f5b2 404 if(t2.read() < 30 &&(newyaw <= 5 && newyaw >= -5)&& (p <= 5 && p >= -5) &&(r <= 5 && r >= -5) && sw == 0) {
57340500084 0:b0ed3674f5b2 405 pc.printf(" *****6666666******* ");
57340500084 0:b0ed3674f5b2 406 a++;
57340500084 0:b0ed3674f5b2 407 pc.printf("\t\t****Tend : %d****\t\t\n",a);
57340500084 0:b0ed3674f5b2 408 sw=1;
57340500084 0:b0ed3674f5b2 409
57340500084 0:b0ed3674f5b2 410 }
57340500084 0:b0ed3674f5b2 411 if(t2.read()>30 && check==1) {
57340500084 0:b0ed3674f5b2 412 if(a==4) {
57340500084 0:b0ed3674f5b2 413 // pc.printf(" You stumbles LV1. ");
57340500084 0:b0ed3674f5b2 414 myledred =1;
57340500084 0:b0ed3674f5b2 415 wait(1);
57340500084 0:b0ed3674f5b2 416 myledred =0;
57340500084 0:b0ed3674f5b2 417 myledblue =1;
57340500084 0:b0ed3674f5b2 418 wait(1);
57340500084 0:b0ed3674f5b2 419 myledblue =0;
57340500084 0:b0ed3674f5b2 420 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 421 wait(2);
57340500084 0:b0ed3674f5b2 422 myledbuzzer=0;
57340500084 0:b0ed3674f5b2 423 wait(0.5);
57340500084 0:b0ed3674f5b2 424 myledbuzzer=1;
57340500084 0:b0ed3674f5b2 425 wait(2);
57340500084 0:b0ed3674f5b2 426 myledbuzzer=0;
57340500084 0:b0ed3674f5b2 427 myledred =1;
57340500084 0:b0ed3674f5b2 428 wait(1);
57340500084 0:b0ed3674f5b2 429 myledred =0;
57340500084 0:b0ed3674f5b2 430 myledblue =1;
57340500084 0:b0ed3674f5b2 431 wait(1);
57340500084 0:b0ed3674f5b2 432 myledblue =0;
57340500084 0:b0ed3674f5b2 433 }
57340500084 0:b0ed3674f5b2 434 if(a>=5) {
57340500084 0:b0ed3674f5b2 435 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 436 wait(0.5);
57340500084 0:b0ed3674f5b2 437 myledbuzzer =0;
57340500084 0:b0ed3674f5b2 438 wait(0.5);
57340500084 0:b0ed3674f5b2 439 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 440 wait(0.5);
57340500084 0:b0ed3674f5b2 441 myledbuzzer =0;
57340500084 0:b0ed3674f5b2 442 wait(0.5);
57340500084 0:b0ed3674f5b2 443 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 444 wait(0.5);
57340500084 0:b0ed3674f5b2 445 myledbuzzer =0;
57340500084 0:b0ed3674f5b2 446 wait(0.5);
57340500084 0:b0ed3674f5b2 447 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 448 wait(0.5);
57340500084 0:b0ed3674f5b2 449 myledbuzzer =0;
57340500084 0:b0ed3674f5b2 450 wait(0.5);
57340500084 0:b0ed3674f5b2 451 myledbuzzer =1;
57340500084 0:b0ed3674f5b2 452 wait(20);
57340500084 0:b0ed3674f5b2 453 myledbuzzer =0;
57340500084 0:b0ed3674f5b2 454 }
57340500084 0:b0ed3674f5b2 455
57340500084 0:b0ed3674f5b2 456 check=0;
57340500084 0:b0ed3674f5b2 457 a=0;
57340500084 0:b0ed3674f5b2 458 t2.reset();
57340500084 0:b0ed3674f5b2 459
57340500084 0:b0ed3674f5b2 460
57340500084 0:b0ed3674f5b2 461 }
57340500084 0:b0ed3674f5b2 462
57340500084 0:b0ed3674f5b2 463 // pc.printf("\tTend : %d\t",a);
57340500084 0:b0ed3674f5b2 464
57340500084 0:b0ed3674f5b2 465
57340500084 0:b0ed3674f5b2 466 }