Diff: actuators.cpp

Revision:

76:0aa90e728e4a

Parent:

74:75be98779124

Child:

79:cf500b63f349

--- a/actuators.cpp	Tue Oct 20 12:14:18 2015 +0200
+++ b/actuators.cpp	Tue Oct 20 12:33:23 2015 +0200
@@ -7,6 +7,8 @@
 #include "Servo.h"
     // functions for controlling the motors
     bool motorsEnable = false;
+    bool safetyOn = false; // start with safety off for calibration
+
 
     double encoder1Counts = 0;
     double encoder2Counts = 0;
@@ -132,7 +134,7 @@
 
 void writeMotors(){
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
-    pidOut = motor2PID.Compute();
+    motor2PID.Compute();
 // write new values to motor's
     if (motor1SetSpeed >= 0 ){ // CCW rotation 
         direction1 = false;
@@ -146,11 +148,9 @@
     }
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
-    
-    double motor2FF = 0.0012*motor2SetSpeed+0.02;
 
     motor1.write(abs(motor1PWM));
-    motor2.write(abs(motor2PWM)+motor2FF);
+    motor2.write(abs(motor2PWM));
 }
 
 void servoControl(){
@@ -189,10 +189,10 @@
     // // set the encoder values for angle.
     // encoder1.setValue(0);
     // encoder2.setValue(0);
+    safetyOn = true; // turn safety on after callibration
 }
 
 
-safetyOn = false; // start with safety off for calibration
 void safety(){
     if (safetyOn){
         motorsEnable = false;