Tried using switch
Dependencies: FastPWM MODSERIAL QEI mbed
Diff: main_demo.cpp
- Revision:
- 2:785737b1cd38
- Parent:
- 1:8cec72aa7728
--- a/main_demo.cpp Wed Oct 31 11:01:10 2018 +0000 +++ b/main_demo.cpp Wed Oct 31 12:32:16 2018 +0000 @@ -37,8 +37,8 @@ double point4y = 0.0; const double x0 = 80.0; //zero x position after homing const double y0 = 141.0; //zero y position after homing -volatile double setpointx = x0; -volatile double setpointy = y0; +double setpointx = x0; +double setpointy = y0; volatile double U1; volatile double U2; @@ -79,18 +79,21 @@ int track = 1; if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){ track = 2; + pc.printf("We gaan nu naar setpoint %i",track); } if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){ track = 3; + pc.printf("We gaan nu naar setpoint %i", track); } if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){ track = 4; + pc.printf("We gaan nu naar setpoint %i",track); } switch(track){ case 1: - setpointx = setpointx + 0.1; - setpointy = setpointy + 0.2; + setpointx = setpointx + (point1x - x0)/150; + setpointy = setpointy + (point1y - y0)/150; break; case 2: @@ -175,6 +178,8 @@ // ----------------------------------------------- void motoraansturing() { + determinedemosetpoints(setpointx, setpointy); + q1_diff = makeAngleq1(setpointx, setpointy); q2_diff = makeAngleq2(setpointx, setpointy);