Roboshark
/
Roboshark_V7
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.
Dependencies: mbed
Fork of
Roboshark_V62
by 
Roboshark
Revision 4:767fd282dd9c, committed 2018-04-24
- Comitter:
- ahlervin
- Date:
- Tue Apr 24 18:16:05 2018 +0000
- Parent:
- 3:e4264cb5b9a9
- Child:
- 5:e715d157ced5
- Commit message:
- Inkl Line Sensor
Changed in this revision
--- a/Fahren.cpp Mon Apr 23 12:19:12 2018 +0000
+++ b/Fahren.cpp Tue Apr 24 18:16:05 2018 +0000
@@ -24,8 +24,8 @@
wegLinks = 1767;
//Geschwindigkeit
- speedRight = 50;
- speedLeft = 50;
+ speedRight = 25;
+ speedLeft = 25;
//Zustand, dass der Roboter nicht gestoppt hat
stopRight = false;
@@ -210,5 +210,99 @@
}
+
+//Implementation der Methode Ziel erreicht
+void Fahren::ziel() {
+
+ //einlesen des aktuellen Encoder standes
+ initialClicksRight = counterRight.read();
+ initialClicksLeft = counterLeft.read();
+ //Anzahl Clicks die der Encoder zählen soll
+ wegRechts = 7050;
+ wegLinks = 7050;
+
+ //Geschwindigkeit
+ speedRight = 80;
+ speedLeft = 80;
+
+ //Zustand, dass der Roboter nicht gestoppt hat
+ stopRight = false;
+ stopLeft = false;
+
+ //Drehrichtung der Motoren
+ controller.setDesiredSpeedRight((speedRight));
+ controller.setDesiredSpeedLeft((speedLeft));
+
+ while(1){
+
+ // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
+ // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
+
+ //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+ if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
+ controller.setDesiredSpeedRight(0);
+ stopRight = true;
+ }
+
+ //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+ if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+ controller.setDesiredSpeedLeft(0);
+ stopLeft = true;
+ }
+
+ if(stopRight == true && stopLeft == true){
+ return;
+ }
+ }
+
+ }
+//Implementation der Methode stopp
+void Fahren::stopp(){
+
+ //einlesen des aktuellen Encoder standes
+ initialClicksRight = counterRight.read();
+ initialClicksLeft = counterLeft.read();
+
+ //Anzahl Clicks die der Encoder zählen soll
+ wegRechts = 0;
+ wegLinks = 0;
+
+ //Geschwindigkeit
+ speedRight = 0;
+ speedLeft = 0;
+
+ //Zustand, dass der Roboter nicht gestoppt hat
+ stopRight = false;
+ stopLeft = false;
+
+ //Drehrichtung der Motoren
+ controller.setDesiredSpeedRight(speedRight);
+ controller.setDesiredSpeedLeft(-(speedLeft));
+
+
+ while(1){
+
+ // printf("Encoderrechts %d \n\r", counterRight.read()); //Debugging Zweck (Putty benutzen!)
+ // printf("Encoderlinks %d \n\r", counterLeft.read()); //Debugging Zweck (Putty benutzen!)
+
+ //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+ if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){
+ controller.setDesiredSpeedRight(0);
+ stopRight = true;
+ }
+
+ //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
+ if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+ controller.setDesiredSpeedLeft(0);
+ stopLeft = true;
+ }
+
+ if(stopRight == true && stopLeft == true){
+ return;
+ }
+
+ }
+
+ }
--- a/Fahren.h Mon Apr 23 12:19:12 2018 +0000
+++ b/Fahren.h Tue Apr 24 18:16:05 2018 +0000
@@ -15,7 +15,9 @@
void geradeaus();
void rechts90();
void rechts180();
- void links90();
+ void links90();
+ void ziel();
+ void stopp();
private:
--- a/IRSensor.cpp Mon Apr 23 12:19:12 2018 +0000
+++ b/IRSensor.cpp Tue Apr 24 18:16:05 2018 +0000
@@ -26,14 +26,27 @@
const int IRSensor :: minIrR = 100; //Noch definieren
const int IRSensor :: minIrL = 100;
const int IRSensor :: minIrF = 100;
+ const float IRSensor :: Period = 0.2;
+
+
// IR Sensor Distanz in mm
-IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) :
-IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){}
+IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) :
+IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){
+ ticker.attach(callback(this, &IRSensor::codeB), Period);
+ this-> ende = ende;
+ ende = 0;
+ }
+
+
-IRSensor::~IRSensor(){}
+IRSensor::~IRSensor(){
+ ticker.detach();
+ }
+
+
float IRSensor::readR() {
@@ -84,5 +97,27 @@
if(disF < minIrR) {
IrF = 1;
} else { IrF = 0; }
- return IrF;
-}
\ No newline at end of file
+ return ende;//IrF;
+}
+
+void IRSensor :: codeB() {
+
+ double line = LineSensor.read();
+ if (line >0.3){ //Line Sensor
+ finish = 1;
+ }else{ finish = 0;
+}
+ if (finish != finishLast){
+ ende = 1;
+}
+ if (line == 1) {
+ ende = 1;
+}
+ finishLast = finish;
+ //printf("Line Sensor ist %d\n",ende);
+ return;
+}
+
+int IRSensor::get_ende(){
+ return ende;
+ }
\ No newline at end of file
--- a/IRSensor.h Mon Apr 23 12:19:12 2018 +0000
+++ b/IRSensor.h Tue Apr 24 18:16:05 2018 +0000
@@ -12,7 +12,7 @@
class IRSensor {
public:
- IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F);
+ IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor);
float disR;
float disL;
@@ -24,6 +24,7 @@
float measR;
float measL;
float measF;
+ double Line;
virtual ~IRSensor();
float readR();
@@ -32,11 +33,15 @@
int codeR();
int codeL();
int codeF();
+ void codeB();
+ int get_ende();
+ int ende;
private:
AnalogIn& IrRight;
AnalogIn& IrLeft;
AnalogIn& IrFront;
+ AnalogIn& LineSensor;
float dis2R;
float dis2L;
float dis2F;
@@ -58,6 +63,11 @@
static const int minIrR;
static const int minIrL;
static const int minIrF;
+ static const float Period;
+ double line;
+ bool finish;
+ bool finishLast;
+ Ticker ticker;
};
--- a/main.cpp Mon Apr 23 12:19:12 2018 +0000
+++ b/main.cpp Tue Apr 24 18:16:05 2018 +0000
@@ -16,7 +16,7 @@
DigitalOut led4(PC_0);
DigitalOut led5(PC_9);
-AnalogIn distance(PB_1);
+AnalogIn LineSensor(PB_1); // Line Sensor
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
@@ -42,9 +42,15 @@
int IrR = 0;
int IrL = 0;
int IrF = 0;
-int caseDrive = 0; //von main Vincent kopiert
+int caseDrive = 0;
+double line = 0;
+bool finish = 0;
+bool finishLast = 0;
+int ende = 0;
+int temp = 0; //von main Vincent kopiert
-IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F); //von main Vincent kopiert
+IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor);
+//IRSensor bottom (IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor); //von main Vincent kopiert
StateMachine stateMachine(IrR, IrL, IrF); //von main Vincent kopiert
@@ -85,10 +91,7 @@
fahren.links90();
wait(1.0f);*/
-
- // IR Sensoren einlesen Programm Vincent
-
-
+ // IR Sensoren einlesen Programm Vincen
float disR = iRSensor.readR(); // Distanz in mm
float disL = iRSensor.readL();
float disF = iRSensor.readF();
@@ -99,30 +102,41 @@
int IrL = iRSensor.codeL();
int IrF = iRSensor.codeF();
/*int caseDrive = stateMachine.drive();*/ //entscheidung welcher Drive Case
-
+ ende = iRSensor.get_ende();
+
+
+
+ /* printf (" ende = %d\n",ende);
+ printf (" finish = %d\n",finish);
+ printf (" finishLast = %d\n",finishLast);
+ printf("line = %f\n", line);
- printf ("IR Right = %d \n", IrR);
+ /* printf ("IR Right = %d \n", IrR);
printf("IR Left = %d\n",IrL);
- printf("IR Front = %d\n",IrF);
+ printf("IR Front = %d\n",IrF);*/
- if((IrR==0) && (IrL==0) && (IrF==1)){
- caseDrive = 2; // Folge: 90 Grad nach rechts drehen
- } else if ((IrR==0) && (IrL==1) && (IrF==0)){
+ if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
caseDrive = 2; // Folge: 90 Grad nach rechts drehen
- } else if ((IrR==0) && (IrL==1) && (IrF==1)){
+ } else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
+ caseDrive = 2; // Folge: 90 Grad nach rechts drehen
+ } else if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 2; // Folge: 90 Grad nach rechts drehen
- } else if ((IrR==1) && (IrL==0) && (IrF==0)){
+ } else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
caseDrive = 1; // Folge: geradeaus fahren
- } else if ((IrR==1) && (IrL==0) && (IrF==1)){
+ } else if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
caseDrive = 3; // Folge: 270 Grad nach rechts drehen
- } else if ((IrR==1) && (IrL==1) && (IrF==0)){
+ } else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
caseDrive = 1; // Folge: geradeaus fahren
- } else if ((IrR==1) && (IrL==1) && (IrF==1)){
+ } else if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 4; // Folge: 180 Grad nach rechts drehen
- } else{ caseDrive=0;
+ } else if ((ende == 1) && (temp == 0)){
+ caseDrive = 5;
+ temp = 1;
+ } else {caseDrive = 0;
}
printf("caseDrive = %d\n",caseDrive);
+ printf("ende = %d\n",ende);
wait (0.5);
@@ -140,11 +154,11 @@
} else if (caseDrive == 4){
fahren.rechts180();
fahren.geradeaus();
- }
-
-
+ } else if (caseDrive == 0){
+ fahren.stopp();
+ } else if(caseDrive == 5){
+ fahren.ziel();
+ fahren.stopp();
+ }
}
}
-
-
-