![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Another one
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture2 by
Diff: main.cpp
- Revision:
- 5:892531e4e015
- Parent:
- 4:34ad002cb646
- Child:
- 6:b526cf83fd4f
diff -r 34ad002cb646 -r 892531e4e015 main.cpp --- a/main.cpp Mon Oct 29 09:55:04 2018 +0000 +++ b/main.cpp Tue Oct 30 08:24:14 2018 +0000 @@ -25,15 +25,15 @@ -DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) +DigitalOut motor1_direction(D4);// draairichting motor 1 (0 is CCW ) PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2 -DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt) +DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW ) -float kp1 = 2; - float kp2 = 2; - float ki1 = 0.5; - float ki2 = 0.5; + float kp1 = 0.2; + float kp2 = 0.2; + float ki1 = 0; + float ki2 = 0; float u1 = 0; float u2 = 0; @@ -61,7 +61,7 @@ Ticker loop_timer; Ticker sample_timer; Ticker sample_timer2; -//HIDScope scope(5); +//HIDScope scope(2); AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength @@ -114,15 +114,14 @@ deg_m1 = deg_m1 + counts_m1*(360/(full_ratio)); deg_m2 = deg_m2 + counts_m2*(360/(full_ratio)); counts_m1_prev = Encoder1.getPulses(); - counts_m2_prev = Encoder2.getPulses(); - + counts_m2_prev = Encoder2.getPulses(); } void scopedata() { - //scope.set(0,emg1_input); // - //scope.set(1,emg1_input); // + //scope.set(0,deg_m1); // + //scope.set(1,deg_m2); // //scope.set(2,emg1_input); // //scope.set(3,emg1_input);// //scope.set(4,filteredsignal1); @@ -209,10 +208,10 @@ } - int deriv1 = deg_m1 - count1; int deriv2 = deg_m2 - count2; + count1 = deg_m1; count2 = deg_m2; @@ -222,12 +221,14 @@ current_state = homing; timer.reset(); state_changed = true; - - deg_m1 = -45; - deg_m2 = -70; + wait(3); + deg_m1 = -2; + deg_m2 = -2; } } +float wu1; +float wu2; void do_state_homing(){ if (state_changed==true) { state_changed = false; @@ -236,21 +237,34 @@ float werror1 = deg_m1 - 0; float werror2 = deg_m2 - 0; - - float wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1; - float wu2 = kp2*werror1 + (u2 + werror1*0.002)*ki2; + if(werror1 > 5){ + wu1 = 1; } + if(werror1 < -5){ + wu1 = -1; } + else{ + wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1; + } - motor1_speed_control = fabs(wu1/200); + if(werror2 > 5){ + wu2 = 1;} + if(werror2 < -5){ + wu2 = -1;} + else{ + wu2 = (kp2*werror2 + (u2 + werror2*0.002)*ki2); + } + + motor1_speed_control = fabs(wu1)/5 + 0.2; + if(wu1 > 0){ motor1_direction = 0;} if(wu1< 0){ motor1_direction = 1;} - motor2_speed_control = fabs(wu2/200); + motor2_speed_control = fabs(wu2)/5 + 0.2; if(wu2 > 0){ motor2_direction = 0;} - if(wu2< 0){ + if(wu2 < 0){ motor2_direction = 1;} @@ -323,8 +337,7 @@ void motor_controller(){ - - + float jacobian[4]; float inv_jacobian[4]; @@ -396,9 +409,9 @@ { // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz - motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration + motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration motor1_speed_control = 0.25;//to make sure the motor will not run at too high velocity - motor2_direction = 0; // set motor 2 to run clockwise (negative) direction + motor2_direction = 0; // set motor 2 to run clockwise direction motor2_speed_control = 0.25; led_red = 0;