Alex Block
/
test
ergwrthsgfhrtxhs
Diff: main.cpp
- Revision:
- 23:550b68587eb5
- Parent:
- 22:5e7356dc1d71
- Child:
- 24:2c7787fc4fde
diff -r 5e7356dc1d71 -r 550b68587eb5 main.cpp --- a/main.cpp Wed Nov 27 19:23:04 2019 +0000 +++ b/main.cpp Fri Dec 06 00:49:17 2019 +0000 @@ -39,18 +39,18 @@ /* Includes */ #include "mbed.h" #include "XNucleoIKS01A2.h" +#include "PID.h" #include <iostream> #include <cmath> -const float MAX_SPEED = 0.0003f; -const float MIN_SPEED = 0.024095f; -//const float MIN_SPEED = 0.5f; +const float MAX_SPEED = 0.00075f; +const float MIN_SPEED = 0.01000f; -const float MAX_DEGREES = 10.0f; +const float MAX_DEGREES = 3.0f; const float MIN_DEGREES = 0.0f; - +float Acc_angle_error = 0; float clamp(float degrees) { @@ -81,7 +81,9 @@ 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); @@ -118,39 +120,70 @@ return radians * 57.2957795131f; } +void calc_accel_error() { + Vec3 vec3; + + for(int i = 0 ; i < 200; i++){ + vec3 = get_accel(); + Acc_angle_error += rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))); + + } + Acc_angle_error /= 200; + +} int main() { - /* Enable all sensors */ magnetometer->enable(); accelerometer->enable(); acc_gyro->enable_x(); acc_gyro->enable_g(); - + + calc_accel_error(); Vec3 vec3; M1_dir = 1; M2_dir = 0; - + float p; M1_step = 0.5f; M2_step = 0.5f; + PID Lpid(1.0, 0.006, 0.003, 0.05); + Lpid.setInputLimits(-45.0,45.0); + Lpid.setOutputLimits(0.0,1.0); + Lpid.setSetPoint(-1.0); + Lpid.setMode(AUTO_MODE); + PID Rpid(1.0, 0.006, 0.003, 0.05); + Rpid.setInputLimits(-45.0,45.0); + Rpid.setOutputLimits(0.0,1.0); + Rpid.setSetPoint(-1.0); + Rpid.setMode(AUTO_MODE); float angle; float period; - - float test = 1.05f; - + float prev = 0; + float L = 0, R = 0; for(;;) { vec3 = get_accel(); - angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))); + int 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; + mean += angle; + wait(.01); + } + mean /= 5; + angle = mean; + period = (degrees_to_period(angle)); - - - -//Change direction. - if(angle > 0) { + Lpid.setProcessValue(angle); + Rpid.setProcessValue(angle * -1); + L = Lpid.compute(); + R = Rpid.compute(); + + p = abs( L-R); + //Change direction. + if(angle < 0) { M1_dir = 0; M2_dir = 1; } else { @@ -158,15 +191,13 @@ M2_dir = 0; } + M1_step.period(unnormalize(p)); + M2_step.period(unnormalize(p)); + + prev = angle; - //Control stepper motor. - out.printf("%f %f %f\r\n", period, angle, degrees_to_period(MAX_DEGREES/test)); - //if(angle < 2.0f || angle > -2.0f) - M1_step.period(period); - M2_step.period(period); - //M1_step.period(0.0003); - //M2_step.period(0.0003); - //M1_step.period(degrees_to_period(MAX_DEGREES/test)); - //M2_step.period(degrees_to_period(MAX_DEGREES/test)); + out.printf("%f %f %f %f\r\n", p, L, R , angle); + + } } \ No newline at end of file