Diff: main.cpp

Revision:

7:167b6dfdefee

Parent:

6:ee243782bf51

--- a/main.cpp	Thu Oct 20 12:07:30 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:10:32 2016 +0000
@@ -24,7 +24,7 @@
 DigitalOut led(LED_RED);
 
 // Set HID scope
-HIDScope    scope(2);
+HIDScope    scope(3);
 
 // Set Encoders for motors
 QEI Motor1_ENC_CW(D11,D10,NC,32);
@@ -35,7 +35,7 @@
 // Constants
 volatile double pwm = 0.0;
 volatile double pwm_new = 0;
-volatile double timer = 0.0;
+volatile double timer = 0.00000000;
 volatile bool button1_value = false;
 volatile bool button2_value = false;
 
@@ -64,6 +64,7 @@
     int zero=0;
     scope.set(0,zero);
     scope.set(1,zero);
+    scope.set(2,zero);
     scope.send();
 }
 
@@ -95,18 +96,18 @@
     {  
         if (MotorValue >=0)
         {
-            motor1_dir=1;
+            motor2_dir=1;
         }
         else
         {
-            motor1_dir=0;
+            motor2_dir=0;
         }
         if (fabs(MotorValue)>1){
-            motor1_pwm.write(1);
+            motor2_pwm.write(1);
         }
         else
         {
-            motor1_pwm.write(abs(MotorValue));
+            motor2_pwm.write(abs(MotorValue));
         }
     }
         
@@ -136,30 +137,37 @@
 
 void MultiSin_motor1()
 {
-    double pwm = pwm_new;
     double f1 = 0.2f;
-    double f2 = 1.0f;
-    double f3 = 5.0f;
-    double f4 = 20.0f;
-    double f5 = 100.0f;
+    double f2 = 0.4f;
+    double f3 = 0.8f;
+    double f4 = 1.6f;
+    double f5 = 3.2f;
+    double f6 = 6.3f;
+    double f7 = 12.6f;
+    double f8 = 25.0f;
+    double f9 = 50.0f;
+    double f10 = 100.0f;
     const double pi = 3.141592653589793238462;
     // read encoder
     double motor1_position = GetPosition(1);
     // set HIDSCOPE values, position, pwm*volt
     scope.set(0, motor1_position);
-    scope.set(1, pwm);
-    // sent HIDScope values
+    scope.set(1, pwm_new);
+    scope.set(2, timer);
     scope.send();
+    // sent HIDScope values
+    
     // calculate the new multisin
-    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
     // devide by the amount of sin waves
-    pwm_new=multisin / 5;
+    pwm_new=multisin / 10.0f;
     // write the motors
-    SetMotor(1, multisin);
+    SetMotor(1, pwm_new);
     // Increase the timer
-    timer = timer + 0.001;
-    if (timer >= (20.0-0.001))
+    timer = timer + 0.001f;
+    if (timer >= (10.00000f))
     {
+        SetMotor(1, 0);
         SinTicker.detach();
         timer = 0;
     }
@@ -167,30 +175,36 @@
 
 void MultiSin_motor2()
 {      
-    double pwm = pwm_new;
     double f1 = 0.2f;
-    double f2 = 1.0f;
-    double f3 = 5.0f;
-    double f4 = 20.0f;
-    double f5 = 100.0f;
+    double f2 = 0.4f;
+    double f3 = 0.8f;
+    double f4 = 1.6f;
+    double f5 = 3.2f;
+    double f6 = 6.3f;
+    double f7 = 12.6f;
+    double f8 = 25.0f;
+    double f9 = 50.0f;
+    double f10 = 100.0f;
     const double pi = 3.141592653589793238462;
     // read encoder
     double motor2_position = GetPosition(2);
     // set HIDSCOPE values, position, pwm*volt
     scope.set(0, motor2_position);
-    scope.set(1, pwm);
-    // sent HIDScope values
+    scope.set(1, pwm_new);
+    scope.set(2, timer);
     scope.send();
+    
     // calculate the new multisin
-    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
     // devide by the amount of sin waves
-    pwm_new=multisin / 5;
-    // write the motor
-    SetMotor(2, multisin);
+    pwm_new=multisin / 10.0f;
+    // write the motors
+    SetMotor(2, pwm_new);
     // Increase the timer
-    timer = timer + 0.001;
-    if (timer >= (20.0-0.001))
+    timer = timer + 0.001f;
+    if (timer >= (10.0000f))
     {
+        SetMotor(2, 0);
         SinTicker.detach();
         timer = 0;
     }
@@ -248,6 +262,8 @@
     {
         // just wait
     }
+    
+    
     pc.printf("\r\n ***************** \r\n");
     pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
     pc.printf("\r\n Pot 1 for motor 1 \r\n");
@@ -262,8 +278,23 @@
     {
         // just wait   
     }
+    // When button is pressed detach PotControl and sendzeros to HIDScope
     PotControlTicker.detach();
+    // Reset the motor PWM values to zero
+    SetMotor(1, 0);
+    SetMotor(2, 0);
+    // wait a bit to make sure all movement is gone
+    wait(0.5);
+    // Reset the QUE encoder values so that the currect position is nog the 0 position
+    Motor1_ENC_CW.reset();
+    Motor1_ENC_CCW.reset();
+    Motor2_ENC_CW.reset();
+    Motor2_ENC_CCW.reset();
+    
+    // Flush the HIDScope with zeros
     SendZerosTicker.attach(&SendZeros,0.001);
+    wait(1);
+    SendZerosTicker.detach();
     
     // Program startup   
     pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
@@ -282,17 +313,18 @@
     pc.printf("\r\n 0 \r\n");
     pc.printf("\r\n Wait for the signal to complete \r\n");
     pc.printf("\r\n Press button 2 too continue afterwards \r\n");
-    SendZerosTicker.detach();
     SinControl1();
 
-    
     while (button2_value == true)
     {
         // just wait   
     }
-    
+    // Flush the HIDScope with zeros
+    SendZerosTicker.attach(&SendZeros,0.001);
+    wait(1);
+    SendZerosTicker.detach();
     
-    SendZerosTicker.attach(&SendZeros,0.001);
+    // Start multisine om motor 2 routine
     pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
     pc.printf("\r\n 10 \r\n");
     wait(5);
@@ -307,7 +339,6 @@
     pc.printf("\r\n 1 \r\n");
     wait(1);
     pc.printf("\r\n 0 \r\n");
-    SendZerosTicker.detach();
     SinControl2();
     wait(23);
     pc.printf("\r\n Program Finished, press reset to restart \r\n");