Changeset 588 for branches/atorf/NXC/MotorControl2/NextGenMotor.nxc

Timestamp:

05/13/09 16:38:49 (4 years ago)

Author:

atorf

Message:

• More progress in NextGenMotor?…

Files:

• branches/atorf/NXC/MotorControl2/NextGenMotor.nxc (modified) (9 diffs)

branches/atorf/NXC/MotorControl2/NextGenMotor.nxc

@@ -43 +43 @@
 
 #define LASTSTEPBRAKINGDEG 2
+#define ENDGAMEBRAKINGSTARTDIST 10
 
 
@@ -133 +134 @@
 
     long absBrakingDist;
+    long brakingStartSpeed;
+    long brakingStartTacho;
 
     long loopStartTick;
@@ -141 +144 @@
     long err_1 = 0;  // prev.   error (t = - T)
     long curPIDPowerUpscaled = absPower * PIDUPSCALING;
+    long newAbsPower;
 
     // speed stuff
@@ -201 +205 @@
         curSpeed = ((MotorTachoCount(port) - TachoCountLog[SpeedLogIndex]) * 1000) / (CurrentTick() - TickCountLog[SpeedLogIndex]);
 
+        //TODO optimize speed logging by using only 2 history values and toggling between them
+
         // record speedlog data to previous index...
         i = SpeedLogIndex - 1;
@@ -234 +240 @@
         
             absBrakingDist = GetAbsBrakingDistFromAbsSpeed(abs(curSpeed));
-            if ( abs(tacholimit -  MotorTachoCount(port)) <= absBrakingDist ) {
+            if ( abs(tachoTarget -  MotorTachoCount(port)) <= absBrakingDist ) {
                 // ok, we're close enough, start braking!
                 curStage = STAGE_BRAKING;
+                // record this for later down (for speed lookup)
+                brakingStartSpeed = curSpeed;
+                brakingStartTacho = powerSgn * (tacholimit - absBrakingDist);
                 //TODO disable SPEED REG if needed!!!
             }//end if
@@ -253 +262 @@
         if (curStage == STAGE_BRAKING) {
 
-            //TODO set x, initSpeed
-
             // shift/calculate all errors for current cycle
             err_1 = err_0;
-            err_0 = curSpeed - GetIdealSpeedFromPos(x, initSpeed, absBrakingDist);
+            err_0 = curSpeed - GetIdealSpeedFromPos(abs(MotorTachoCount(port) - brakingStartTacho), brakingStartSpeed, absBrakingDist);
 
 
@@ -264 +271 @@
                                 + (KP * err_1);
 
+
             //TODO clip these values!
+            newAbsPower = curPIDPowerUpscaled / PIDUPSCALING;
+            if (newAbsPower > 100) {
+                newAbsPower = 100;
+            } else if (newAbsPower < 1) {
+                newAbsPower = 1;
+            }//end if
+            
+            //finally set new power!
+            UpdatePower(port, sgn * newAbsPower);
+            
+            
 
             #ifdef ENABLEDEBUGGING_REMOTE
@@ -282 +301 @@
         waitEndTick = loopStartTick + LOOP_DURATION;
         while (CurrentTick() < waitEndTick) {
-            // check tacho count...
-        
+
+            if ( abs(tachoTarget - MotorTachoCount(port)) <  ENDGAMEBRAKINGSTARTDIST) {
+               curStage = STAGE_ENDGAME;
+               break;
+            }//end if
+            
         }//end while
 
@@ -293 +316 @@
         
             // fast inner loop now!
+            // don't forget TIMEOUT or something, avoid possible lock up
             
         }//end if