Alexander Welz
BERTL16 LineDrive
Revision 0:68b692bb433c, committed 2019-01-24
- Comitter:
- WelzAlex
- Date:
- Thu Jan 24 21:06:58 2019 +0000
- Commit message:
- BERTL16Drive
Changed in this revision
diff -r 000000000000 -r 68b692bb433c Serial_HL.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Serial_HL.lib Thu Jan 24 21:06:58 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/hollegha2/code/Serial_HL/#b958bdf108cf
diff -r 000000000000 -r 68b692bb433c dracuDrive.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/dracuDrive.cpp Thu Jan 24 21:06:58 2019 +0000
@@ -0,0 +1,30 @@
+#include "dracuDrive.h"
+#include "mbed.h"
+
+
+dracuMotor::dracuMotor(PinName MotorEN, PinName MotorFOR , PinName MotorREV):
+ motorEnabled(MotorEN), motorForward(MotorFOR), motorReverse(MotorREV)
+ {
+ motorEnabled = 0.0;
+ }
+void dracuMotor::Speed(float speed)
+{
+ if (speed <0)
+ {
+ motorForward =0;
+ motorReverse =1;
+ motorEnabled = (-1)*speed;
+ }
+ else
+ {
+ motorForward =1;
+ motorReverse =0;
+ motorEnabled = speed;
+ }
+ _speed = speed;
+}
+
+dracuMotor::operator float ()
+{
+ return _speed;
+}
\ No newline at end of file
diff -r 000000000000 -r 68b692bb433c dracuDrive.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/dracuDrive.h Thu Jan 24 21:06:58 2019 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+
+
+class dracuMotor
+{
+ public:
+ dracuMotor(PinName MotorEN, PinName MotorFOR , PinName MotorREV);
+ operator float();
+ void Speed(float speed);
+ private:
+ float _speed;
+ PwmOut motorEnabled;
+ DigitalOut motorForward;
+ DigitalOut motorReverse;
+
+};
+
+class AnalogInHL2 : public AnalogIn {
+ public:
+ AnalogInHL2(PinName pin) : AnalogIn(pin) {}
+ int Read()
+ { return read_u16()>>6; }
+};
diff -r 000000000000 -r 68b692bb433c main.cpp
--- /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
diff -r 000000000000 -r 68b692bb433c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jan 24 21:06:58 2019 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10 \ No newline at end of file