code for the speed robot
Dependencies: MPU6050-DMP mbed PololuQTRSensors vl53l0x
_var.h@7:fd80a0d11658, 2019-09-30 (annotated)
- Committer:
- deepanaishtaweera174
- Date:
- Mon Sep 30 13:16:04 2019 +0000
- Revision:
- 7:fd80a0d11658
- Parent:
- 3:9b2f15b0d47b
added motor control code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
deepanaishtaweera174 | 0:c3a27228c31b | 1 | I2C i2c(PB_9,PB_8); |
deepanaishtaweera174 | 1:b188e27eb7da | 2 | Timer t; |
deepanaishtaweera174 | 3:9b2f15b0d47b | 3 | |
deepanaishtaweera174 | 3:9b2f15b0d47b | 4 | VL53L0X Sensor3(&i2c,&t); //Right Sensor |
deepanaishtaweera174 | 3:9b2f15b0d47b | 5 | VL53L0X Sensor2(&i2c,&t); //Left Sensor |
deepanaishtaweera174 | 3:9b2f15b0d47b | 6 | VL53L0X Sensor1(&i2c,&t); //Front Sensor |
deepanaishtaweera174 | 3:9b2f15b0d47b | 7 | int right_Distance; |
deepanaishtaweera174 | 3:9b2f15b0d47b | 8 | int left_Distance; |
deepanaishtaweera174 | 3:9b2f15b0d47b | 9 | int front_Distance; |
deepanaishtaweera174 | 0:c3a27228c31b | 10 | |
deepanaishtaweera174 | 0:c3a27228c31b | 11 | // Gyro Variables |
deepanaishtaweera174 | 0:c3a27228c31b | 12 | MPU6050 mpu; |
deepanaishtaweera174 | 0:c3a27228c31b | 13 | volatile bool mpuInterrupt = false; |
deepanaishtaweera174 | 0:c3a27228c31b | 14 | unsigned long savedMillis; |
deepanaishtaweera174 | 0:c3a27228c31b | 15 | bool gyroTurnMode = false; |
deepanaishtaweera174 | 7:fd80a0d11658 | 16 | int zAngle = 0; |
deepanaishtaweera174 | 7:fd80a0d11658 | 17 | int xAngle = 0; |
deepanaishtaweera174 | 7:fd80a0d11658 | 18 | int preZAngle = 0; |
deepanaishtaweera174 | 7:fd80a0d11658 | 19 | int preXAngle = 0; |
deepanaishtaweera174 | 7:fd80a0d11658 | 20 | int startAngle = 0; |
deepanaishtaweera174 | 0:c3a27228c31b | 21 | unsigned long preGyroMillis = 0; |
deepanaishtaweera174 | 0:c3a27228c31b | 22 | |
deepanaishtaweera174 | 0:c3a27228c31b | 23 | // MPU control/status vars |
deepanaishtaweera174 | 0:c3a27228c31b | 24 | bool dmpReady = false; // set true if DMP init was successful |
deepanaishtaweera174 | 0:c3a27228c31b | 25 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU |
deepanaishtaweera174 | 0:c3a27228c31b | 26 | uint8_t devStatus; |
deepanaishtaweera174 | 0:c3a27228c31b | 27 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) |
deepanaishtaweera174 | 0:c3a27228c31b | 28 | uint16_t fifoCount; |
deepanaishtaweera174 | 0:c3a27228c31b | 29 | uint8_t fifoBuffer[64]; |
deepanaishtaweera174 | 0:c3a27228c31b | 30 | |
deepanaishtaweera174 | 0:c3a27228c31b | 31 | // orientation/motion vars |
deepanaishtaweera174 | 0:c3a27228c31b | 32 | Quaternion q; // [w, x, y, z] quaternion container |
deepanaishtaweera174 | 0:c3a27228c31b | 33 | VectorInt16 aa; // [x, y, z] accel sensor measurements |
deepanaishtaweera174 | 0:c3a27228c31b | 34 | VectorInt16 aaReal; // [x, y, z] |
deepanaishtaweera174 | 0:c3a27228c31b | 35 | VectorInt16 aaWorld; // [x, y, z] |
deepanaishtaweera174 | 0:c3a27228c31b | 36 | VectorFloat gravity; // [x, y, z] gravity vector |
deepanaishtaweera174 | 0:c3a27228c31b | 37 | float euler[3]; // [psi, theta, phi] Euler angle container |
deepanaishtaweera174 | 0:c3a27228c31b | 38 | float ypr[3]; // [yaw, pitch, roll] |