Diff: main.cpp

Revision:

3:c524afedf863

Parent:

2:92593c9d6146

Child:

4:05c5da1c8b08

--- a/main.cpp	Fri Oct 21 08:44:48 2016 +0000
+++ b/main.cpp	Mon Oct 24 09:00:05 2016 +0000
@@ -48,8 +48,8 @@
 volatile double x = 0.0;                             // beginpositie x en y
 volatile double y = 305.5;
 volatile const double pi = 3.14159265359; 
-volatile double Theta1Gear = 1.0/3.0*pi;             // Beginpositie op 60 graden
-volatile double Theta2Gear = 1.0/3.0*pi;
+volatile double Theta1Gear = 60.0;             // Beginpositie op 60 graden
+volatile double Theta2Gear = 60.0;
 volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
 volatile double Theta2 = Theta2Gear*42/12;
 double Fr_Speed_Theta = 100.0;              // frequentie in Hz waarmee theta wordt uigerekend
@@ -172,7 +172,7 @@
     else if (y < 50) {
         y = 50;         // GOKJE, UITPROBEREN
     }
-    //pc.printf("x = %f, y = %f\r\n", x, y);
+    pc.printf("x = %f, y = %f\r\n", x, y);
       
     //scope.set(2,x);
     //scope.set(3,y);
@@ -205,7 +205,7 @@
     double a = 50.0; 
     double Bar = 200.0;    // lengte van de armen 
     double Prev_Theta1_Gear = Theta1Gear; 
-    double MaxThetaGear = 100.0*pi/180;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
+    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
     
     // Hoek berekenen van het grote tandwiel (gear)
     if (x > -a) {
@@ -217,6 +217,7 @@
     else {  // als x=-a
         Theta1Gear = 0.5*pi - acos(Dia1/Bar);
     }
+    Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
              
     // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
     if (Theta1Gear >= MaxThetaGear) {    // is de maximale hoek in radialen
@@ -228,14 +229,14 @@
     // omrekenen van grote tandwiel naar motortandwiel
     Theta1 = Theta1Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
         
-    //pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear);
+    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear);
 }
 
 void CalcTheta2 (double Dia2, double Prev_x, double Prev_y) {
     double a = 50.0; 
     double Bar = 200.0;    // lengte van de armen 
     double Prev_Theta2_Gear = Theta2Gear; 
-    double MaxThetaGear = 100.0*pi/180;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
+    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
     
     // Hoek berekenen van het grote tandwiel (gear)
     if (x < a) {
@@ -247,6 +248,7 @@
     else {  // als x=a
         Theta2Gear = 0.5*pi - acos(Dia2/Bar);
     }
+    Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
     
         // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
     if (Theta2Gear >= MaxThetaGear) {
@@ -258,7 +260,7 @@
     // omrekenen van grote tandwiel naar motortandwiel
     Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
 
-    //pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear);
+    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear);
 }
 
 void CalculationsForTheta () {
@@ -310,7 +312,7 @@
    // sample_timer.attach(&sample, 0.002);
 
     // ticker voor hoek berekenen aanzetten 
-    TickerCalculationsForTheta.attach(&CalculationsForTheta,0.002);
+    TickerCalculationsForTheta.attach(&CalculationsForTheta,0.002); 
    
     //empty loop, sample() is executed periodically
     while(1) {