![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ergwrthsgfhrtxhs
Revision 25:5049c26a7e73, committed 2019-12-09
- Comitter:
- block011
- Date:
- Mon Dec 09 05:31:37 2019 +0000
- Parent:
- 24:2c7787fc4fde
- Commit message:
- Final publish
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Dec 06 17:37:07 2019 +0000 +++ b/main.cpp Mon Dec 09 05:31:37 2019 +0000 @@ -1,40 +1,3 @@ -/** - ****************************************************************************** - * @file main.cpp - * @author CLab - * @version V1.0.0 - * @date 2-December-2016 - * @brief Simple Example application for using the X_NUCLEO_IKS01A1 - * MEMS Inertial & Environmental Sensor Nucleo expansion board. - ****************************************************************************** - * @attention - * - * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** -*/ /* Includes */ #include "mbed.h" @@ -52,6 +15,16 @@ float Acc_angle_error = 0; +PwmOut M1_step(D3); +DigitalOut M1_dir(D2); + +PwmOut M2_step(D5); +DigitalOut M2_dir(D4); + +Serial out(USBTX, USBRX); + + + float clamp(float degrees) { if(degrees < MIN_DEGREES) return MIN_DEGREES; @@ -59,15 +32,11 @@ else return degrees; } - - float degrees_to_period(float degrees) { return (((MAX_SPEED - MIN_SPEED)/(MAX_DEGREES)) * clamp(std::abs(degrees)) + MIN_SPEED); } - - struct Vec3 { float x, y, z; }; @@ -80,23 +49,10 @@ static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro; static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer; - float unnormalize(float t){ return (MIN_SPEED - (t * ( MIN_SPEED - MAX_SPEED))); } -PwmOut M1_step(D3); -DigitalOut M1_dir(D2); - -PwmOut M2_step(D5); -DigitalOut M2_dir(D4); - - -Serial out(USBTX, USBRX); - - - - Vec3 get_accel() { Vec3 vec3; @@ -129,7 +85,6 @@ } Acc_angle_error /= 200; - } @@ -143,27 +98,35 @@ calc_accel_error(); Vec3 vec3; - + float angle; M1_dir = 1; M2_dir = 0; float p; M1_step = 0.5f; M2_step = 0.5f; - PID Lpid(2.0, 0, 0.00, 0.05); + + //pid setup for left leaning + PID Lpid(1.0, 0.001, 0.00, 0.05); Lpid.setInputLimits(-45.0,45.0); Lpid.setOutputLimits(0.0,1.0); Lpid.setSetPoint(0.0); Lpid.setMode(AUTO_MODE); - PID Rpid(2.0, 00, 0.0, 0.05); + //pid setup for right leaning + PID Rpid(1.0, 0.006, 0.0, 0.05); Rpid.setInputLimits(-45.0,45.0); Rpid.setOutputLimits(0.0,1.0); Rpid.setSetPoint(0.0); Rpid.setMode(AUTO_MODE); - float prev = 0; + float L = 0, R = 0; + + // main while loop for(;;) { + //get xyz data vec3 = get_accel(); + + //calculates mean angle from 5 seperate readings float mean = 0; for (int i = 0; i < 5; i++) { angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))) - Acc_angle_error; @@ -173,7 +136,7 @@ mean /= 5; angle = mean; - + //calculate pid Lpid.setProcessValue(angle); Rpid.setProcessValue(angle * -1); L = Lpid.compute(); @@ -192,9 +155,7 @@ M1_step.period(unnormalize(p)); M2_step.period(unnormalize(p)); - - prev = angle; - + //serial print out.printf("%f %f %f %f\r\n", p, L, R , angle);