Roboshark
/
Roboshark_V10
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_V9
by 
Roboshark
main.cpp
- Committer:
- ahlervin
- Date:
- 2018-05-15
- Revision:
- 15:8e8b23d4abb4
- Parent:
- 14:feafcee53fed
File content as of revision 15:8e8b23d4abb4:
/*Roboshark V10
main.cpp
Erstellt: J. Blunschi
geändert: V.Ahlers
V.5.18
main zu Robishark
*/
#include <mbed.h>
#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Fahren.h"
DigitalOut myled(LED1);
DigitalOut led0(PC_8); //Ready
DigitalOut ledLinksAktiv(PC_6); // Links Aktiv
DigitalOut ledRechtsAktiv(PB_12); // Rechts Aktiv
DigitalOut led3(PA_7);
DigitalOut led4(PC_0);
DigitalOut led5(PC_9);
DigitalOut enable(PC_1);
DigitalOut bit0(PH_1);
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);
AnalogIn IrRight(PC_3); // IR Sensoren
AnalogIn IrLeft (PC_5);
AnalogIn IrFront(PC_2);
AnalogIn LineSensor(PB_1); // Line Sensor
DigitalIn button (USER_BUTTON); //Auswählen ob Rechts oder Links methode
float disR = 0;
float disL = 0;
float disF = 0;
float dis2R = 0;
float dis2L = 0;
float dis2F = 0;
int IrR = 0;
int IrL = 0;
int IrF = 0;
int caseDrive = 0;
double line = 0;
bool finish = 0;
bool finishLast = 0;
int ende = 0;
int temp = 0;
float SpeedR = 0;
float SpeedL = 0;
int links = 0;
int linksLast = 0;
int skip = 0;
int BL = 1;
//von main Vincent kopiert
DigitalOut enableMotorDriver(PB_2);
DigitalIn motorDriverFault(PB_14);
DigitalIn motorDriverWarning(PB_15);
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);
EncoderCounter counterLeft(PB_6, PB_7);
EncoderCounter counterRight(PA_6, PC_7);
IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor, button); //von main Vincent kopiert
Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Fahren fahren(controller, counterLeft, counterRight, iRSensor); //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
int main(){
enable = 1;
enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
while(1) {
if(button == linksLast){
links = 1;
linksLast = links;
} else { links = 0;
}
// IR Sensoren einlesen
float disR = iRSensor.readR(); // Distanz in mm
float disL = iRSensor.readL();
float disF = iRSensor.readF();
dis2R = disR;
dis2L = disL;
dis2F = disF;
int IrR = iRSensor.codeR(); // min Distanz unterschritten = 1
int IrL = iRSensor.codeL();
int IrF = iRSensor.codeF();
ende = iRSensor.get_ende(); // Line erkennt = 1
BL = button;
if (button == 0){
temp = 0;
ende = 0;
skip = 0;
}
if ((links == 0) && (skip == 0)){
wait (1);
skip = 1;
} else { if ((links == 1) && ( skip == 0)){
wait (1);
skip = 1;
}
}
/* 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 Left = %d\n",IrL);
printf("IR Front = %d\n",IrF);*/
switch(links) {
case 0: // State machine für methode Rechts
if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
caseDrive = 2;
} else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
caseDrive = 2;
} else if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 2;
} else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
caseDrive = 1;
} else if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
caseDrive = 3;
} else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
caseDrive = 1;
} else if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 4;
} else if ((ende == 1) && (temp == 0)){
caseDrive = 5;
temp = 1;
} else {caseDrive = 0;
}
break;
case 1: //State Machine für Methode Links
if((IrR==0) && (IrL==0) && (IrF==0) && (ende == 0)){
caseDrive = 3;
} else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
caseDrive = 3;
} else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
caseDrive = 1;
} else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
caseDrive = 1;
} else if ((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
caseDrive = 3;
} else if ((IrR==1) && (IrL==0)&& (IrF==1) && (ende == 0)){
caseDrive = 3;
} else if ((IrR==0)&& (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 2;
} else if ((IrR==1)&& (IrL==1) && (IrF==1) && (ende == 0)){
caseDrive = 4;
} else if ((ende == 1) && (temp == 0)){
caseDrive = 5;
temp = 1;
} else {caseDrive = 0;
}
}
printf("ende = %d\n",ende);
wait (0.01);
if(caseDrive == 1){ // Aufrufen der verschiedenen fahr funktionen
fahren.geradeausG();
} else if (caseDrive == 2){
fahren.rechts90();
fahren.geradeausG();
} else if (caseDrive == 3){
fahren.links90();
fahren.geradeausG();
} else if (caseDrive == 4){
fahren.rechts180();
fahren.geradeausG();
} else if (caseDrive == 0){
fahren.stopp();
} else if(caseDrive == 5){
fahren.ziel();
fahren.stopp();
}
}
}