Diff: actuators.cpp

Revision:

3:47c76be6d402

Parent:

2:95ba9f6f0128

Child:

4:80e2280058ed

diff -r 95ba9f6f0128 -r 47c76be6d402 actuators.cpp
--- a/actuators.cpp	Thu Oct 01 15:36:55 2015 +0200
+++ b/actuators.cpp	Mon Oct 05 16:01:21 2015 +0200
@@ -1,8 +1,25 @@
 // functions for controlling the motors
 
 void motorInit(){
-    PwmOut motor1(motorpin1);
-    PwmOut motor2(motorpin2);
+    // Initialze motors
+    PwmOut motor1(motor1PWMPin);
+    PwmOut motor2(motor2PWMPin);
+
+    // Set motor direction pins.
+    DigitalOut motor1Dir(motor1DirPin);
+    DigitalOut motor2Dir(motor2DirPin);
+
+    // Set initial direction
+    motor1Dir.write(direction1);
+    motor2Dir.write(direction2);
+
+    // Set motor PWM period
+    motor1.period(1/pwm_frequency);
+    motor2.period(1/pwm_frequency);
+
+    // Initialize encoders
+    Encoder encoder1(enc1A, enc1B);
+    Encoder encoder2(enc2A, enc2B);
 
     // create PID instances for motors
     // PID pidname(input, output, setpoint, kp, ki, kd, direction)
@@ -14,8 +31,8 @@
     PIDmotor2.SetMode(AUTOMATIC);
 
     // set limits for PID output to avoid integrator build up.
-    PIDmotor1.SetOutputLimits(-1f, 1f);
-    PIDmotor2.SetOutputLimits(-1f, 1f);
+    PIDmotor1.SetOutputLimits(-1, 1);
+    PIDmotor2.SetOutputLimits(-1, 1);
     }
 
 
@@ -23,14 +40,17 @@
     if(motorEnable){  // only run motors if switch is enabled
 
     // get encoder positions
+        motor1Pos = encoder1.getPosition();
+        motor2Pos = encoder2.getPosition();
+
         // check if motor's are within rotational boundarys
     // calculate  encoder speeds
-        motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime);
-        motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime);
+        motorSpeed1=(motor1Pos-prevMotor1Pos)/(time.read()-prevTime);
+        motorSpeed2=(motor2Pos-prevMotor2Pos)/(time.read()-prevTime);
 
         // store current positions and time
-        prevMotorPos1 = motorPos1;
-        prevMotorPos2 = motorPos2;
+        prevMotor1Pos = motor1Pos;
+        prevMotor2Pos = motor2Pos;
         prevTime = time.read();
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
@@ -59,7 +79,7 @@
     
 void pumpControl(){
     if (pumpButton == HIGH){
-        // write pumpPin High
+        // write pumpPin High 
         }else{
         // write pumpPin Low    
         }