Diff: main.cpp

Revision:

3:f70ec68723df

Parent:

2:a8ee608177ae

--- a/main.cpp	Thu Nov 01 10:58:42 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:33:27 2018 +0000
@@ -15,20 +15,30 @@
 DigitalOut motor2direction(D4);
 PwmOut motor2control(D5);
 
-InterruptIn button1(D10);
+InterruptIn casebutton1(PTA4);                                                  //Button for switching the cases
+InterruptIn casebutton2(PTC6);                                                  //Button for switching the cases
 
+InterruptIn button1(D10);                                                       //Button for switching x-axes and for getting zero and max
+InterruptIn button2(D11); 
 Serial pc(USBTX, USBRX);
 
 // Definitie constanten
 double L0 = 0.09;
-double K_p1 = 0.5;
-double K_p2 = 0.5;
-double p_desired_x = 0.04;
-double p_desired_y = 0.13;
+double K_p1 = 2.0;
+double K_p2 = 7.0;
+double v_x = 0.02; // speed in m/s
+double v_y = 0.02; // speed in m/s
+
+double motorValue1;
+double motorValue2;
+
 double r_pulley = 0.015915;
 double pi = 3.141592653589793;
 double gearratio = 3.857142857;
 
+Ticker motor1ticker;
+Ticker motor2ticker;
+
 // Definitie variabelen
 double angle_trans;
 double translatie;
@@ -41,16 +51,45 @@
 double motor2_pwm;
 double error_length_angle;
 double error_angle;
-
+double p_desired_x = 0.002;
+double p_desired_y = 0.002;
 double p_current_x;
 double p_current_y;
 
+void setMotor1() {
+    motor1control.write(motorValue1);
+    }
+ 
+void setMotor2() {
+    motor2control.write(motorValue2);
+    }
+
 void EncoderFunc() 
 {
     angle_trans = Encoder1.getPulses() * 0.0857142857*0.0174532925;                  // Translation [rad]
     translatie = angle_trans * r_pulley;           // Translatie arm [m]
     angle = Encoder2.getPulses() * 0.0857142857*0.0174532925/gearratio;                        // Angle arm [rad]
     length = translatie+L0;
+  
+    if (button2 <=0.5){ //Button ingedrukt
+        p_desired_x = p_desired_x + v_x*0.02;
+        }
+    else {
+        p_desired_x = p_desired_x;
+        }
+    if (button1 <=0.5){ //Button ingedrukt
+        p_desired_y = p_desired_y + v_y*0.02;
+        }
+    else {
+        p_desired_y = p_desired_y;
+        }
+    
+    if (casebutton2 == 0) {
+        v_x = -v_x;
+        }
+    if (casebutton1 == 0) {
+        v_y = -v_y;
+        }
 
     p_current_x = (length)*cos(angle)-L0;
     p_current_y = (length)*sin(angle);
@@ -68,26 +107,29 @@
     motor2_pwm = K_p2*error_angle;
     
     //Motor 1 (Translatie)
-    if (error_length_angle >= 0) {
+    if (motor1_pwm >= 0) {
         motor1direction = false; //Positieve bewegingsrichting (clockwise, towards end)
         }
     else {
         motor1direction = true; // Negatieve bewegingsrichting
         }
-     motor1control.write(fabs(motor1_pwm));
+     motorValue1 = fabs(motor1_pwm);
         
     //Motor 2 (Rotatie)
-    if (error_angle >= 0){
+    if (motor2_pwm >= 0){
         motor2direction = false; // counterclockwise (arm clockwise)
         }
     else {
         motor2direction = true; // clockwise (arm counterclockwise)
         }
-    motor2control.write(fabs(motor2_pwm));
+    motorValue2 = fabs(motor2_pwm);
 }    
 
 int main() {
     
+motor1ticker.attach(&setMotor1, 0.02);
+motor2ticker.attach(&setMotor2, 0.02);
+
 EncoderTicker.attach(&EncoderFunc, 0.02);
 pc.baud(115200);
 motor1direction = false;
@@ -99,5 +141,7 @@
     pc.printf("angle = %f, length = %f \r\n", angle*180/pi, length);
     pc.printf("x = %f, y = %f \r\n", p_current_x, p_current_y);
     pc.printf("motor1_pwm = %f, motor2_pwm = %f \r\n", motor1_pwm, motor2_pwm);
+    pc.printf("p_desired_x = %f, p_desired_y = %f \r\n", p_desired_x, p_desired_y);
+
     }
 }
\ No newline at end of file