Diff: main.cpp

Revision:

27:1d55ebab6214

Parent:

26:5e4b329defec

--- a/main.cpp	Wed Aug 20 08:36:08 2014 +0000
+++ b/main.cpp	Fri Aug 22 06:38:17 2014 +0000
@@ -1,10 +1,10 @@
 #include "mbed.h"
-#include "RawSerial.h"
 #include "EquatorStrutController.h"
+#include "LightWeightSerialTransmit.h"
 
 Timer RunningTime;
 
-RawSerial pc(P1_27, P1_26);
+LWSerialTX SerialOutput(115200);
 
 double vMax = 300;
 double vStep = 50;
@@ -15,8 +15,6 @@
 double pLinStep = pStep * G1 / Glin;
 double TargetPwm;
 
-char counter = 0;
-
 EquatorStrut strut;
 
 void LinearizePower(double power)
@@ -58,122 +56,32 @@
 
 double PosError = 0;    //This has been defined here as it's being used in the serial transmit function
 
-void SerialOut(double outputValue)
-{
-    int outChar = 0;
-    
-    if (outputValue < 0.0)
-    {
-        pc.putc('-');
-        outputValue *= -1.0;
-    }
-    if (outputValue >= 1000.0)
-    {
-        outChar = outputValue / 1000;
-        pc.putc(outChar + 48);
-        outputValue -= outChar * 1000.0;
-    }
-    if (outputValue >= 100.0)
-    {
-        outChar = outputValue / 100;
-        pc.putc(outChar + 48);
-        outputValue -= outChar * 100.0;
-    }
-    else if(outChar > 0)
-    {
-        pc.putc('0');
-    }
-    if (outputValue >= 10.0)
-    {
-        outChar = outputValue / 10;
-        pc.putc(outChar + 48);
-        outputValue -= outChar * 10.0;
-    }
-    else if(outChar > 0)
-    {
-        pc.putc('0');
-    }
-    if (outputValue >= 1.0)
-    {
-        outChar = outputValue;
-        pc.putc(outChar + 48);
-        outputValue -= outChar;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (outputValue >= 0.1)
-    {
-        pc.putc('.');
-        outChar = outputValue * 10;
-        pc.putc(outChar + 48);
-        outputValue -= (double)outChar / 10.0;
-    }
-    else
-    {
-        pc.putc('.');
-        pc.putc('0');
-    }
-    if (outputValue >= 0.01)
-    {
-        outChar = outputValue * 100;
-        pc.putc(outChar + 48);
-        outputValue -= (double)outChar / 100.0;
-    }
-    else
-    {
-        pc.putc('0');
-    }
-    if (outputValue >= 0.001)
-    {
-        outChar= outputValue * 1000;
-        pc.putc(outChar + 48);
-    }
-}
-
 double PosKpGain = 0.0;
 double PosKiGain = 0.0;
 double PosKdGain = 0.0;
 
 void SerialTransmit()
 {    
-    SerialOut(RunningTime.read()); 
-    
-    pc.putc('\t');   
-    
-    SerialOut(strut.GetPosition());
+    SerialOutput.Transmit(RunningTime.read()); 
+    SerialOutput.Delimiter(LWSerialTX::Tab);   
     
-    pc.putc('\t');
+    SerialOutput.Transmit(strut.GetPosition());
+    SerialOutput.Delimiter(LWSerialTX::Tab);
         
-    SerialOut(strut.CurrentPower());
-    
-    pc.putc('\t');
+    SerialOutput.Transmit(strut.CurrentPower());
+    SerialOutput.Delimiter(LWSerialTX::Tab);
     
-    SerialOut(strut.CurrentSpeed());
-    
-    pc.putc('\t');
-    
-    SerialOut(PosError);
+    SerialOutput.Transmit(strut.CurrentSpeed());
+    SerialOutput.Delimiter(LWSerialTX::Tab);
     
-    pc.putc('\t');
-    
-    SerialOut(PosKpGain);
+    SerialOutput.Transmit(PosError);
+    SerialOutput.Delimiter(LWSerialTX::Tab);
     
-    pc.putc('\t');
-    
-    pc.putc(10);
-    pc.putc(13);
+    SerialOutput.Transmit(PosKpGain);
+    SerialOutput.NewLine();
 }
 
-void SerialNewFile()
-{
-    pc.putc(28);
-    pc.putc(10);
-    pc.putc(13);
-}
-
-double SetPoint = 150.0; //Target Position in Millimeters
+double SetPoint = 200.0; //Target Position in Millimeters
 
 double PosProError;
 double PosIntError;
@@ -234,7 +142,7 @@
         TargetPwm = -1.0;
     }
     
-    strut.SetPower(TargetPwm);
+    LinearizePower(TargetPwm);
     
     errorcounter++;
 
@@ -247,14 +155,10 @@
 }
 
 int main()
-
 {    
     RunningTime.start();
     
-    pc.baud(115200);
-    
     strut.Home();
-    //Enable();
     
     errorcounter = 0;
     PosPreviousError[errorcounter]=0;
@@ -263,17 +167,15 @@
     while(strut.IsEnabled())
     {
         int counter = 0;
-        //double pow = 0.0;
-        //while(pow < 1.0)
         PosKpGain = 0.0;
+        
         while(PosKpGain < 1.0)
         {
             counter++;
             
-            //pow += 0.05;
             PosKpGain += 0.01;
           
-            SerialNewFile();
+            SerialOutput.NewFile();
           
             RunningTime.reset();
                                 
@@ -285,25 +187,7 @@
                 
                 Controller();
             }
-                
-            //SetPower(pow);
             
-            //while(position < 21000 && RunningTime.read()-iterationStart < 30.0)
-            //{
-            //    SerialTransmit();
-            //}
-            
-            //iterationStart = RunningTime.read();
-            //SerialNewFile();
-            
-            //SetPower(-pow);
-            
-            //while(position > 1000 && RunningTime.read()-iterationStart < 30.0)
-            //{
-            //    SerialTransmit();
-            //}
-                
-            //SerialNewFile();
             strut.Home();
             
             if (counter == 10)