Diff: main.cpp

Revision:

0:68b692bb433c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jan 24 21:06:58 2019 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "dracuDrive.h"
+#include "Serial_HL.h"
+
+SerialBLK pc(USBTX, USBRX);
+SvProtocol ua0(&pc);
+void CommandHandler();
+BusOut boardPow(P2_13, P2_5, P2_2); // B16/17
+AnalogInHL2 ls1(P1_9), ls2(P0_16), ls3(P1_3), ls4(P0_23), ls5(P0_12);
+DigitalIn dummy(P0_15);
+dracuMotor mL(P1_19,P2_15,P2_14); 
+dracuMotor mR(P2_19,P2_20,P2_21);
+
+int GetPin(PinName aName)
+{
+  return (aName >> PIN_SHIFT) & 0x0000003F;
+}
+
+int GetPort(PinName aName)
+{
+  return (aName >> PORT_SHIFT) & 0x0000003F;
+}
+
+int main(void) 
+{
+    boardPow =3;
+    float abweichung = 0;
+
+    pc.format(8,SerialBLK::None,1); pc.baud(115200); // 115200
+    ua0.SvMessage("LineSensor!"); // Meldung zum PC senden
+    
+   float i=0.3;
+   ua0.acqON = 1;
+   while(1)
+   {
+        CommandHandler();
+        mL.Speed(i);
+        mR.Speed(i);
+
+       
+        abweichung = (ls1.Read()*(-8) +ls2.Read()*(-1)+ ls1.Read() *(0) +ls4.Read()*(1) +ls5.Read()*(10))/10000.0;
+        
+            if(abweichung > 0.9)
+            {
+                mR.Speed(0);
+                mL.Speed(0);
+                wait_ms(250);
+                mR.Speed(-i);
+                mL.Speed(i);
+                wait_ms(250);
+            }
+            else if(abweichung < -0.9)
+            {
+                mR.Speed(0);
+                mL.Speed(0);
+                wait_ms(250);
+                mR.Speed(i);
+                mL.Speed(-i);
+                wait_ms(250);
+            }
+            else
+            {
+                mR.Speed(i - abweichung+0);
+                mL.Speed(i + (abweichung+0));
+            }
+        
+        
+        
+        if( ua0.acqON ) {
+            // nur wenn vom PC aus das Senden eingeschaltet wurde
+            // wird auch etwas gesendet
+            ua0.WriteSvI16(1, ls1.Read());  // Wert von ls1 auf Kanal 1 an SvVis
+            ua0.WriteSvI16(2, ls2.Read());
+            ua0.WriteSvI16(3, ls3.Read());
+            ua0.WriteSvI16(4, ls4.Read());
+            ua0.WriteSvI16(5, ls5.Read());
+            ua0.WriteSV(6, abweichung);
+        }    
+    }
+}
+
+
+void CommandHandler()
+{
+  uint8_t cmd;
+  int16_t idata1, idata2;
+    // Fragen ob überhaupt etwas im RX-Reg steht
+  if( !pc.IsDataAvail() )
+    return;
+    // wenn etwas im RX-Reg steht
+    // Kommando lesen
+    cmd = ua0.GetCommand();
+}
\ No newline at end of file