Alex Block
/
test
ergwrthsgfhrtxhs
Diff: main.cpp
- Revision:
- 21:b0e9b4d19d4d
- Parent:
- 20:babcf777607b
- Child:
- 22:5e7356dc1d71
diff -r babcf777607b -r b0e9b4d19d4d main.cpp --- a/main.cpp Fri Nov 22 23:54:51 2019 +0000 +++ b/main.cpp Sat Nov 23 00:55:46 2019 +0000 @@ -42,11 +42,18 @@ #include <iostream> #include <cmath> -const float MAX_SPEED = .002; -const float MIN_SPEED = .003; -const float MAX_DEGREES = 3.0; -const float MIN_DEGREES = .5; -const float MAX_TILT = (MIN_SPEED - MAX_SPEED) / MAX_DEGREES; +const float MAX_SPEED = 0.0014f; +const float MIN_SPEED = 0.1f; + +const float MAX_DEGREES = 5.0f; +const float MIN_DEGREES = 0.0f; + + + +float degrees_to_period(float degrees) +{ + return (((MAX_SPEED - MIN_SPEED)/(MAX_DEGREES)) * std::abs(degrees) + MIN_SPEED); +} @@ -65,13 +72,16 @@ -DigitalOut M1_step(D3); +PwmOut M1_step(D3); DigitalOut M1_dir(D2); -DigitalOut M2_step(D5); +PwmOut M2_step(D5); DigitalOut M2_dir(D4); +Serial out(USBTX, USBRX); + + Vec3 get_accel() @@ -109,52 +119,36 @@ acc_gyro->enable_g(); Vec3 vec3; - int numOfSteps; M1_dir = 1; M2_dir = 0; + + M1_step = 0.5f; + M2_step = 0.5f; - float given; + float angle; + float period; for(;;) { vec3 = get_accel(); - given = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))); - - numOfSteps = given / 0.055f; - - - if (std::abs(given) > MIN_DEGREES) { -//Change direction. - if(given > 0) { - M1_dir = 0; - M2_dir = 1; - } else { - M1_dir = 1; - M2_dir = 0; - } - - float stepTime; - given = std::abs(given); - - if (given > MAX_DEGREES){ - stepTime = MAX_SPEED; - } - else{ - stepTime = (MAX_TILT * given) + MIN_SPEED; - } - //Control stepper motor. - for(int i = 0; i < numOfSteps; ++i) { - M2_step = 1; - M1_step = 1; - wait(0.0014); - M1_step = 0; - M2_step = 0; - wait(0.0014); - } - } + angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))); + period = (degrees_to_period(angle)); +//Change direction. + if(angle > 0) { + M1_dir = 0; + M2_dir = 1; + } else { + M1_dir = 1; + M2_dir = 0; + } + + //Control stepper motor. + out.printf("%f %f\r\n", period, angle); + M1_step.period(period); + M2_step.period(period); } } \ No newline at end of file