zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/main.cpp@0:d267b248eff4, 2017-03-11 (annotated)

Committer:
beacon
Date:
Sat Mar 11 10:14:00 2017 +0000
Revision:
0:d267b248eff4
Child:
1:388c915756f5
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:d267b248eff4 1 #include "mbed.h"
beacon 0:d267b248eff4 2 #include "Robot.h"
beacon 0:d267b248eff4 3
beacon 0:d267b248eff4 4 #include <cstdlib>
beacon 0:d267b248eff4 5
beacon 0:d267b248eff4 6 #define NEAR 0.1f //Used for distance Sensors. If they're to near to a wall -> turn
beacon 0:d267b248eff4 7 #define LEFT //Arrayindex of the left Sensor
beacon 0:d267b248eff4 8 #define FWD //Arrayindex of the forward Sensor
beacon 0:d267b248eff4 9 #define RIGHT //Arrayindex of the right Sensor
beacon 0:d267b248eff4 10
beacon 0:d267b248eff4 11
beacon 0:d267b248eff4 12 //Robot related:
beacon 0:d267b248eff4 13 PwmOut left(PA_8);
beacon 0:d267b248eff4 14 PwmOut right(PA_3);
beacon 0:d267b248eff4 15
beacon 0:d267b248eff4 16 DigitalOut powerSignal(PB_2);
beacon 0:d267b248eff4 17 DigitalIn errorSignal(PB_14);
beacon 0:d267b248eff4 18 DigitalIn overtemperatur(PB_15);
beacon 0:d267b248eff4 19
beacon 0:d267b248eff4 20 Robot bot( &left, &right, &powerSignal );
beacon 0:d267b248eff4 21
beacon 0:d267b248eff4 22 //DistanceSensors related:
beacon 0:d267b248eff4 23 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:d267b248eff4 24
beacon 0:d267b248eff4 25 AnalogIn sensorVoltage(PB_1);
beacon 0:d267b248eff4 26 DigitalOut enable(PC_1);
beacon 0:d267b248eff4 27 DigitalOut bit0(PH_1);
beacon 0:d267b248eff4 28 DigitalOut bit1(PC_2);
beacon 0:d267b248eff4 29 DigitalOut bit2(PC_3);
beacon 0:d267b248eff4 30 //Robot::DistanceSensors sensors[6];
beacon 0:d267b248eff4 31
beacon 0:d267b248eff4 32 //LED related:
beacon 0:d267b248eff4 33 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:d267b248eff4 34
beacon 0:d267b248eff4 35 void initialiseDistanceSensors(){
beacon 0:d267b248eff4 36 for( int ii = 0; ii<6; ++ii) {
beacon 0:d267b248eff4 37 bot.sensors[ii].init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
beacon 0:d267b248eff4 38
beacon 0:d267b248eff4 39 enable = 1;
beacon 0:d267b248eff4 40 }
beacon 0:d267b248eff4 41 }
beacon 0:d267b248eff4 42
beacon 0:d267b248eff4 43 void fahralgorithmus(){
beacon 0:d267b248eff4 44
beacon 0:d267b248eff4 45 int rando = 0; //Will be used for rdandom actions.
beacon 0:d267b248eff4 46
beacon 0:d267b248eff4 47 while( 1 ) { //Remember to change the indices. Also make into a seperate function
beacon 0:d267b248eff4 48
beacon 0:d267b248eff4 49 if ( bot.sensors[0] < NEAR ) { //Left Sensor
beacon 0:d267b248eff4 50 while ( bot.sensors[0] < NEAR ) {
beacon 0:d267b248eff4 51 bot.turnRight();
beacon 0:d267b248eff4 52 }
beacon 0:d267b248eff4 53 }
beacon 0:d267b248eff4 54
beacon 0:d267b248eff4 55 else if ( bot.sensors[1] < NEAR ) { //Front Sensor
beacon 0:d267b248eff4 56 rando = rand() % 2;
beacon 0:d267b248eff4 57 while ( bot.sensors[1] < NEAR ) {
beacon 0:d267b248eff4 58 bot.turnAround(rando);
beacon 0:d267b248eff4 59 }
beacon 0:d267b248eff4 60 }
beacon 0:d267b248eff4 61
beacon 0:d267b248eff4 62 else if ( bot.sensors[2] < NEAR ) { //Right Sensor
beacon 0:d267b248eff4 63 while ( bot.sensors[2] < NEAR ) {
beacon 0:d267b248eff4 64 bot.turnLeft();
beacon 0:d267b248eff4 65 }
beacon 0:d267b248eff4 66 }
beacon 0:d267b248eff4 67
beacon 0:d267b248eff4 68 else {
beacon 0:d267b248eff4 69 bot.drive();
beacon 0:d267b248eff4 70 }
beacon 0:d267b248eff4 71
beacon 0:d267b248eff4 72 }
beacon 0:d267b248eff4 73 }
beacon 0:d267b248eff4 74
beacon 0:d267b248eff4 75 /* * /
beacon 0:d267b248eff4 76
beacon 0:d267b248eff4 77
beacon 0:d267b248eff4 78 int main(){
beacon 0:d267b248eff4 79 while( 1 ){
beacon 0:d267b248eff4 80 bot.drive();
beacon 0:d267b248eff4 81 }
beacon 0:d267b248eff4 82 return 0;
beacon 0:d267b248eff4 83 }
beacon 0:d267b248eff4 84 /* */
beacon 0:d267b248eff4 85
beacon 0:d267b248eff4 86
beacon 0:d267b248eff4 87 /* */
beacon 0:d267b248eff4 88 int main()
beacon 0:d267b248eff4 89 {
beacon 0:d267b248eff4 90 initialiseDistanceSensors();
beacon 0:d267b248eff4 91
beacon 0:d267b248eff4 92 while( 1 ){
beacon 0:d267b248eff4 93 for(int i=0; i<6; i++){
beacon 0:d267b248eff4 94 if(bot.sensors[i] > 0.05f){
beacon 0:d267b248eff4 95 leds[i] = 1;
beacon 0:d267b248eff4 96 }
beacon 0:d267b248eff4 97 else{
beacon 0:d267b248eff4 98 leds[i] = 0;
beacon 0:d267b248eff4 99 }
beacon 0:d267b248eff4 100 }
beacon 0:d267b248eff4 101 }
beacon 0:d267b248eff4 102
beacon 0:d267b248eff4 103 return 0;
beacon 0:d267b248eff4 104 }
beacon 0:d267b248eff4 105 /* */