juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
Diff: main.cpp
- Revision:
- 10:91173ed1e841
- Parent:
- 9:4f594927cff3
- Child:
- 11:c7e27de26ac0
diff -r 4f594927cff3 -r 91173ed1e841 main.cpp --- a/main.cpp Thu Nov 01 08:30:56 2018 +0000 +++ b/main.cpp Thu Nov 01 16:50:47 2018 +0000 @@ -12,7 +12,7 @@ int sensor_sensitivity = 32; int gear_ratio = 131; float full_ratio = gear_ratio*sensor_sensitivity*4; - +double pi = 3.141592653589793238462643383279502884; DigitalIn but1(D2); QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A @@ -153,6 +153,8 @@ int gain = 3; + int sign1 = 0.5; + int sign2 = 1; void do_forward(){ @@ -368,7 +370,7 @@ threshold2 = 0.5 * max2; } - if (timer.read() > 5 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { + if (timer.read() > 1 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) { current_state = operational; timer.reset(); state_changed = true; @@ -427,9 +429,9 @@ float inv_jacobian[4]; jacobian[0] = L1; - jacobian[1] = (L2*cos(deg_m2))+L1; + jacobian[1] = (L2*cos(deg_m2/180*pi))+L1; jacobian[2] = -L0; - jacobian[3] = -L0 - (L2*sin(deg_m2)); + jacobian[3] = -L0 - (L2*sin(deg_m2/180*pi)); float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2])); @@ -444,8 +446,8 @@ - ref_q1 = ref_q1 + 0.002*ref_v1; - ref_q2 = ref_q2 + 0.002*ref_v2; + ref_q1 = ref_q1 + 0.002*ref_v1 * sign1; + ref_q2 = ref_q2 + 0.002*ref_v2 * sign2; error1 = deg_m1 - ref_q1; error2 = deg_m2 - ref_q2;