rushilwiz/engine-software
1
0
Fork 0
mirror of https://github.com/PotentiaRobotics/engine-software.git synced 2025-04-17 18:40:18 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity

i love p code

Browse Source
This commit is contained in:
FluffyCube9343 2022-10-29 14:31:51 -04:00
parent 26b3148706
commit 9ac6887f98
1 changed files with 19 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
P_control_Code/P_control_Code.ino
View File

@ -1,48 +1,22 @@
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) // NOTES
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net> // if the end of the axle is facing you/up:
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // rot counterclcokwise increases IMU Yaw (positively to 3.14)
// rot clockwise decreases IMU Yaw (negatively to -3.14)
// sending 1500+x command to servo rotates it counterclockwise
// sending 1500-x command to servo rotates it clockwise
// command is sent as 1500 - (rotation)*coefficient
// //
// Changelog: //
// 2019-07-08 - Added Auto Calibration and offset generator //
// - and altered FIFO retrieval sequence to avoid using blocking code //
// 2016-04-18 - Eliminated a potential infinite loop //
// 2013-05-08 - added seamless Fastwire support //
// - added note about gyro calibration //Upd. 10/29 2:25 PM
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
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.
===============================================
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project // for both classes must be in the include path of your project
@ -89,7 +63,7 @@ MPU6050 mpu(0x69); // <-- use for AD0 high
double tic = millis(); double tic = millis();
double goal = 0.0; double goal = 0.0;
double kp = 1; double kp = 750;
double ki = 1; double ki = 1;
double kd = 1; double kd = 1;
@ -264,7 +238,7 @@ myservo.attach(3);
Serial.println(F(")")); Serial.println(F(")"));
} }
// configure LED for output // configure LED for outputj
pinMode(LED_PIN, OUTPUT); pinMode(LED_PIN, OUTPUT);
} }
@ -300,9 +274,9 @@ void loop() {
properr = goal-ypr[0]; properr = goal-ypr[0];
//interr += properr * (toc-tic); //interr += properr * (toc-tic);
Serial.println(properr*kp); Serial.println(properr*kp);
int toRot = properr*kp;
int servoVal = map(properr * kp, -3.15 * kp, 3.15 * kp, 1000, 2000); int Rot = max(min(1500-toRot,2000),1000);
myservo.writeMicroseconds(servoVal); myservo.writeMicroseconds(Rot);