Final project for CS335 By Maxwell Poster and Jeffrey Resnik
Dependencies: mbed MPU6050_template ledControl2 USBDevice
Revision 14:e6e3099e3b3b, committed 2020-12-02
- Comitter:
- mposter
- Date:
- Wed Dec 02 00:19:20 2020 +0000
- Parent:
- 13:e9fe7586ab0c
- Commit message:
- Mini cleanup
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r e9fe7586ab0c -r e6e3099e3b3b main.cpp --- a/main.cpp Mon Nov 30 16:39:57 2020 +0000 +++ b/main.cpp Wed Dec 02 00:19:20 2020 +0000 @@ -1,37 +1,4 @@ -/* Getting Pitch and Roll Angles from MPU6050 -* -* @author: Baser Kandehir -* @date: July 16, 2015 -* @license: MIT license -* -* Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in -* all copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -* THE SOFTWARE. -* -* @description of the program: -* -* First of all most of the credit goes to Kris Winer for his useful MPU6050 code. -* I rewrite the code in my way using class prototypes and my comments. This program -* can be a starter point for more advanced projects including quadcopters, balancing -* robots etc. Program takes accelerometer and gyroscope data from the MPU6050 registers -* and calibrates them for better results. Then uses this data is to obtain pitch and roll -* angles and writes these angles to the terminal which mbed is connected to. +/* * * @connections: *-------------------------------------------------------------- @@ -48,9 +15,6 @@ * GND -----------> GND of MPU6050 * VOUT (3.3 V) --> VCC of MPU6050 *--------------------------------------------------------------- - - -* Note: For any mistakes or comments, please contact me. */ #include "mbed.h" @@ -69,17 +33,13 @@ #define SCROLLDOWN_LENGTH 5 //! Variable debug #define DEBUG -/* */ - -/* Defined in the MPU6050.cpp file */ - //I2C i2c(p9,p10); // setup i2c (SDA,SCL) +/* */ //! Instance pc de classe Serial //Serial pc(USBTX,USBRX); // default baud rate: 9600 //! Instance mpu6050 de classe MPU6050 MPU6050 mpu6050; // class: MPU6050, object: mpu6050 -//! instance toggler1 de classe Ticker -//! instance filer de classe Ticker + // Ticker // A Ticker is used to call a function at a recurring interval. // You can use as many separate Ticker objects as you require. @@ -93,6 +53,7 @@ void toggle_led2(); void compFilter(); +//Not used by us, but required to apply the filter from the MPU lib float pitchAngle = 0; float rollAngle = 0; @@ -141,7 +102,10 @@ int main() { - //Imported + + //Begin initialize********************************************************* + + //Imported https://os.mbed.com/users/remig/code/i2c_MPU6050//file/369106aa267a/main.cpp/ ////pc.baud(9600); // baud rate: 9600 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading wait(1); @@ -154,14 +118,20 @@ wait_ms(500); //End imported + + //Perform our own sampling calibration for(int i = 0; i < QUEUE_LENGTH; i++){ filter.attach(&compFilter, 0.005); updateDeques(); } + + //Calculate biases calibrateGyro(); calibrateAccel(); + //End intialize*********************************************************** + ////////pc.printf("Bias x: %.3f | Bias z: %.3f\r\n",gyroBiasx,gyroBiasz); //wait(2.5); @@ -173,7 +143,7 @@ //////////pc.printf(" _____________________________________________________________ \r\n"); //////////pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az); - printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz); + //printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz); //////////pc.printf("|_____________________________________________________________ \r\n\r\n"); // //X AXIS GYRO @@ -194,21 +164,11 @@ filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) - //#ifdef DEBUG - //////////pc.printf(" [Debug On] \r\n"); - //mpu6050.complementaryFilter(&pitchAngle, &rollAngle); - //#endif - //////////pc.printf(" _______________\r\n"); - //////////pc.printf("| Pitch: %.3f \r\n",pitchAngle); - //////////pc.printf("| Roll: %.3f \r\n",rollAngle); - //////////pc.printf("|_______________\r\n\r\n"); - - //wait(1); updateDeques(); processMovement(); checkClicks(); - //wait(2.5); + //wait(2.5); } } @@ -216,6 +176,7 @@ void checkClicks(){ //Imported + //https://os.mbed.com/users/guqinchen/code/P4_IMU//file/044740372a78/main.cpp/ static bool preRightClick = false; if (leftClick) @@ -270,8 +231,10 @@ //Convert to radians float sampleRadians = sample*PI/180.0; + //Compute distance to move float deltaX = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians); + //Cast to int16_t int16_t sending = 0 + deltaX; printf("x axis MOVING: %.3f, sending: %i\r\n",deltaX,sending); @@ -295,12 +258,15 @@ //////pc.printf("sample (Radians) y: %.3f \r\n", sampleRadians); + //Compute distance to move float deltaZ = SCREEN_DISTANCE*RESOLUTION*tan(sampleRadians); //Apply multipliers to enable mouse acceleration if(deltaZ > 0){ deltaZ*=1.75; }else{ + //If the amount to move z was negative, it tended to be stronger than the amount to move the z in the positive + // so we weighted negative z deltas less than positive ones deltaZ*=1.5; }