Christian Weiß
/
Diplomarbeit_MW_CW
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 1:dfa0f59e8d2c, committed 2018-11-15
- Comitter:
- Wizo
- Date:
- Thu Nov 15 17:57:01 2018 +0000
- Parent:
- 0:afeca64a6543
- Commit message:
- Diplomarbeit_MW_CW
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 17 17:19:45 2018 +0000
+++ b/main.cpp Thu Nov 15 17:57:01 2018 +0000
@@ -1,4 +1,3 @@
-
#include "MMouse18.h"
#include "Serial_HL.h"
@@ -6,35 +5,109 @@
// SerialBLK pc(p28, p27);
SvProtocol ua0(&pc);
+//Funktionen
void CommandHandler();
void CheckButton();
+void ForewardUntilXing();
+void NinetyDegreesLeft();
+void TurnAround();
+void CorrectToRight();
+void CorrectToLeft();
+bool CheckLeftSensor();
+bool CheckLeftFrontSensor();
+bool CheckRightSensor();
+bool CheckRightFrontSensor();
+float speed = -0.3;
-Motor mL(p22,p24,p23); Motor mR(p26, p21, p25);
-Encoder encL(p15,p16); Encoder encR(p30,p29);
+
+
+int SensorLeft = 0;
+int SensorFrontLeft = 0;
+int SensorRight = 0;
+int SensorFrontRight = 0;
+
-DigitalIn sw(p5);
+Motor mL(p22,p24,p23); Motor mR(p26, p21, p25);//(Enable, Input Rückwärts, Input Vorwärts)#####255 Encoder-tics = 90 Grad Kurve
+Encoder encL(p15,p16); //(Motor-Encoder Links A, Motor-Encoder Rechts A)
+Encoder encR(p30,p29);//(Motor-Encoder Links B, Motor-Encoder Rechts B)
+
+DistSens sla(p13,p20); // Sensor Links außen (Emitter, Receiver)
+DistSens sli(p14,p19); // Sensor Links innen
+DistSens sra(p12,p17); // Sensor rechts außen
+DistSens sri(p11,p18); // Sensor rechts innen
+
+// DigitalIn sw(p5); geht an p5 nicht
BusOut leds(LED1,LED2,LED3,LED4);
int main(void)
{
+
+ leds=0;
pc.format(8,SerialBLK::None,1);
pc.baud(115200);
mL.SetPow(0); mR.SetPow(0);
encL.Init(ENC_A,ENC_RISE); encR.Init(ENC_A,ENC_RISE);
- ua0.SvMessage("MotTest1");
+ ua0.SvMessage("MotTest7");
Timer stw;
stw.start();
while(1) {
- CheckButton();
- CommandHandler();
+// CheckButton();
+ CommandHandler();
+
+
+ SensorLeft = sla.Measure();
+ SensorFrontLeft = sli.Measure();
+ SensorRight = sra.Measure();
+ SensorFrontRight = sri.Measure();
+
+
+ ForewardUntilXing(); //#############################################################
+
if( stw.read_ms()>10 ) {
stw.reset();
if( ua0.acqON ) {
- ua0.WriteSvI16(1, encL.cnt);
- ua0.WriteSvI16(2, encR.cnt);
+
+// leds =1;
+// wait_ms(3000);
+// leds =2;
+// wait_ms(3000);
+// leds =3;
+// wait_ms(3000);
+// leds =4;
+// wait_ms(3000);
+// leds =5;
+// wait_ms(3000);
+// leds =6;
+// wait_ms(3000);
+// leds =7;
+// wait_ms(3000);
+// leds =8;
+// wait_ms(3000);
+// leds =9;
+// wait_ms(3000);
+// leds =10;
+// wait_ms(3000);
+
+// SensorLeft = sla.Measure();
+// SensorFrontLeft = sli.Measure();
+// SensorRight = sra.Measure();
+// SensorFrontRight = sri.Measure();
+/*
+ ua0.WriteSvI16(1, sla.Measure());//Sensor links außen in ProcVis anzeigen
+ ua0.WriteSvI16(2, sli.Measure());
+ ua0.WriteSvI16(3, sra.Measure());
+ ua0.WriteSvI16(4, sri.Measure());
+*/
+ ua0.WriteSvI16(1, SensorLeft);//Sensor links außen in ProcVis anzeigen
+ ua0.WriteSvI16(2, SensorFrontLeft);
+ ua0.WriteSvI16(3, SensorRight);
+ ua0.WriteSvI16(4, SensorFrontRight);
+ ua0.WriteSvI16(5, encL.cnt);//Encoder Motor links in ProcVis anzeigen
+ ua0.WriteSvI16(6, encR.cnt);
+
}
}
}
@@ -48,19 +121,152 @@
return;
cmd = ua0.GetCommand();
if( cmd==2 ) {
- mL.SetPow(ua0.ReadF()); mR.SetPow(ua0.ReadF());
- ua0.SvMessage("Set Pow");
+ leds=2;
+ mL.SetPow(ua0.ReadF()); mR.SetPow(ua0.ReadF());//Motor starten. Eingabe: 2 0,3 0,3
+ ua0.SvMessage("cmd: 2, Set Pow");
}
if( cmd==3 ) {
- encL.cnt=encR.cnt=0;
- ua0.SvMessage("Reset Cnt");
+ leds=3;
+ encL.cnt=encR.cnt=0;//Encoder zurücksetzen
+ ua0.SvMessage("cmd: 3, Reset Cnt");
}
+
+ if( cmd==4 )
+ {
+ leds=4;
+ ua0.SvMessage("cmd: 4");
+ }
+/* sla.useLED=ua0.ReadI16();
+ sli.useLED=ua0.ReadI16();
+ sra.useLED=ua0.ReadI16();
+ sri.useLED=ua0.ReadI16();
+ ua0.SvMessage("LED On/Off");
+ }*/
}
void CheckButton()
{
- if( sw==1 )
+ /* if( sw==1 )
leds=0xF;
else
- leds=0;
-}
\ No newline at end of file
+ leds=0; */
+}
+
+
+void ForewardUntilXing()
+{
+// while (CheckLeftFrontSensor() != 1 && CheckLeftSensor() != 1)
+// {
+
+ if (SensorFrontLeft > 1000 || SensorFrontRight > 1000)
+ {
+ TurnAround();
+// mL.SetPow(0); mR.SetPow(0);
+// wait_ms(10000);
+ }
+
+ mL.SetPow(speed); mR.SetPow(speed);//geradeaus
+ if(SensorLeft > 1500 /*(zu weit Links)*/)
+ {
+ CorrectToRight();
+ }
+ if(SensorRight > 1500 /* (zu weit Rechts)*/)
+ {
+ CorrectToLeft();
+ }
+
+// }
+}
+
+
+void NinetyDegreesLeft()
+{
+ for (int i = 0; i <= 10; i++)
+ {
+ mL.SetPow(speed); mR.SetPow(-0.1);//Linkskurve
+ wait_ms(100);
+ }
+
+}
+
+
+//bool CheckLeftSensor()
+//{
+// if (SensorLeft < 1000)
+// {
+// CorrectToRight();
+// return 1;
+// }
+// else
+// {
+// return 0;//Abstand zur Wand korrekt
+// }
+//}
+
+
+
+bool CheckLeftFrontSensor()
+{
+ if (SensorFrontLeft > 0)
+ {
+ return 1;
+ }
+ else
+ {
+ return 0;
+ }
+}
+
+
+//bool CheckRightSensor()
+//{
+//
+// if (SensorFrontRight < 1000)
+// {
+// CorrectToLeft();
+// return 1;
+// }
+// else
+// {
+// return 0;//Abstand zur Wand korrekt
+// }
+//
+//}
+
+
+bool CheckRightFrontSensor()
+{
+ if (SensorRight > 0)
+ {
+ return 1;
+ }
+ else
+ {
+ return 0;
+ }
+}
+
+
+void CorrectToLeft()
+{
+ mL.SetPow(-0.1); mR.SetPow(speed);//nach links
+ wait_ms(30);
+ mL.SetPow(speed); mR.SetPow(speed);//wieder geradeaus
+}
+
+
+void CorrectToRight()
+{
+ mL.SetPow(speed); mR.SetPow(-0.1);//nach rechts
+ wait_ms(30);
+ mL.SetPow(speed); mR.SetPow(speed);//wieder geradeaus
+}
+
+
+void TurnAround()
+{
+ encL.cnt=encR.cnt=0;//Encoder zurücksetzen
+ while(encR.cnt<=25588)
+ mL.SetPow(speed); mR.SetPow(0.3);//nach rechts
+
+}