Merge pull request #12 from mit-drl/position_pid

Position pid

e-vent.ino

@@ -42,7 +42,7 @@ float BPM_MAX = 30;
 float IE_MIN = 1;
 float IE_MAX = 4;
 float VOL_MIN = 100;
-float VOL_MAX = 450; // 900; // For full 
+float VOL_MAX = 700; // 900; // For full 
 
 //Setup States
 States state;
@@ -64,6 +64,15 @@ RoboClaw roboclaw(&serial,10000);
 #define qpps 3000
 int motorPosition = 0;
 
+// position PID values for PG188
+#define pKp 8.0
+#define pKi 0.0
+#define pKd 0.0
+#define kiMax 10.0
+#define deadzone 0
+#define minPos 0
+#define maxPos 1000
+
 // LCD Screen
 const int rs = 12, en = 11, d4 = 10, d5 = 9, d6 = 8, d7 = 7;
 LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
@@ -131,20 +140,17 @@ int readEncoder() {
 // goToPosition goes to a desired position at the given speed,
 void goToPosition(int pos, int vel){
   bool valid = readEncoder();
-  int diff = pos - motorPosition;
 
-  if(diff < 0){
-    vel = -vel; //want to reverse vel if you need to go backwards
-    diff = abs(diff);
-  }
+  int accel = 10000;
+  int deccel = 10000;
 
   if(valid){
-    roboclaw.SpeedDistanceM1(address,vel,diff, 1);
+    roboclaw.SpeedAccelDeccelPositionM1(address,accel,vel,deccel,pos,1);
     if(DEBUG){
       Serial.print("CmdVel: ");
       Serial.print(vel);
       Serial.print("\tCmdDiff: ");
-      Serial.println(diff);
+      Serial.println(pos);
     }
   }
   else{
@@ -164,7 +170,8 @@ void setup() {
   setState(PREHOME_STATE); // Initial state
   roboclaw.begin(38400); // Roboclaw
   roboclaw.SetM1MaxCurrent(address, 10000); // Current limit is 10A
-  roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); // Set PID Coefficients
+  roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); // Set Velocity PID Coefficients
+  roboclaw.SetM1PositionPID(address,pKp,pKi,pKd,kiMax,deadzone,minPos,maxPos); // Set Position PID Coefficients
   roboclaw.SetEncM1(address, 0); // Zero the encoder
 
   if(DEBUG){
@@ -205,8 +212,9 @@ void loop() {
       enteringState = false;
       goToPosition(Volume, Vin);
     }
-
-    if(millis()-stateTimer > Tin*1000 || abs(motorPosition - Volume) < goalTol){
+
+    // Consider checking we reached the destination for fault detection
+    if(millis()-stateTimer > Tin*1000){
       setState(PAUSE_STATE);
     }
   }
@@ -218,7 +226,6 @@ void loop() {
     }
     if(millis()-stateTimer > pauseTime){
       pressure.set_plateau();
-
       setState(EX_STATE);
     }
   }
@@ -230,9 +237,6 @@ void loop() {
       enteringState = false;
       goToPosition(0, Vex);
     }
-
-    if(abs(motorPosition) < goalTol)
-      roboclaw.ForwardM1(address,0);
 
     if(millis()-stateTimer > Tex*1000){
       pressure.set_peak_and_reset();
@@ -269,9 +273,7 @@ void loop() {
     }
 
     if(digitalRead(HOME_PIN) == HIGH) {
-      // Stop motor
-      roboclaw.ForwardM1(address,0);
-      delay(pauseHome);
+      delay(pauseHome); // Wait for things to settle
       roboclaw.SetEncM1(address, 0); // Zero the encoder
       setState(POSTHOME_STATE);
 
@@ -283,14 +285,15 @@ void loop() {
     //Entering
     if(enteringState){
       enteringState = false;
-      roboclaw.ForwardM1(address,Vhome);
+      goToPosition(bagHome, Vhome); // Stop motor
     }
 
     if(abs(motorPosition - bagHome) < goalTol){
       // Stop the motor and home encoder
       roboclaw.ForwardM1(address,0);
-      delay(pauseHome);
+      delay(pauseHome); // Wait for things to settle
       roboclaw.SetEncM1(address, 0); // Zero the encoder
+      goToPosition(0, Vhome); // Stop motor
       setState(IN_STATE);
     }
   }