Diff: main.cpp

Revision:

2:d1805e7d46fb

Parent:

1:a33723b70582

Child:

3:e693c65b04de

--- a/main.cpp	Tue Aug 05 13:09:58 2014 +0000
+++ b/main.cpp	Thu Aug 07 07:11:59 2014 +0000
@@ -211,7 +211,7 @@
         pc.putc(outChar + 48);
     }
     
-    pc.putc(',');
+    pc.putc('\t');
     
     outChar = 0;
     
@@ -269,7 +269,7 @@
         pc.putc(outChar + 48);
     }
     
-    pc.putc(',');
+    pc.putc('\t');
     outChar = 0;
     
     if (tempPow < 0.0)
@@ -315,7 +315,7 @@
         pc.putc(outChar + 48);
     }
     
-    pc.putc(',');
+    pc.putc('\t');
     outChar = 0;
     
     if (tempSpeed < 0.0)
@@ -391,7 +391,21 @@
     RGHCosInterrupt.enable_irq();
 }
 
+double TargetSpeed=0;
+double Error =0;
+double KpTerm;
+double ErrorDer;
+double ErrorInt;
+
+double KpGain = 0.0;
+double KiGain = 0.0;
+double KdGain = 0.0;
+
+int counter;
+double PreviousError [10];
+
 int main()
+
 {
     RGHSinInterrupt.rise(&RGHSinHandler);
     RGHCosInterrupt.rise(&RGHCosHandler);
@@ -404,10 +418,27 @@
     
     Home();
     
-    while(1)
+    //while(1)
+    
+    counter = 0;
+    
+    PreviousError[counter]=0;
+    
+    if (GetSpeed() <= TargetSpeed)
+    
+    {
+        Error = TargetSpeed - GetSpeedd();
+        
+        KpTerm = Error * KpGain;
+        
+        ErrorDer = (Error - PreviousError[counter])
+    }
+    
     {        
         SerialTransmit();
         
+        
+    
         switch(counter)
         {
         case 0: