zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/main.cpp@9:ac362674c480, 2017-05-02 (annotated)

Committer:
beacon
Date:
Tue May 02 08:39:35 2017 +0000
Revision:
9:ac362674c480
kl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 9:ac362674c480 1 #include "mbed.h"
beacon 9:ac362674c480 2 #include "Robot.h"
beacon 9:ac362674c480 3 #include "Declarations.h"
beacon 9:ac362674c480 4
beacon 9:ac362674c480 5 #include <cstdlib>
beacon 9:ac362674c480 6
beacon 9:ac362674c480 7 //DistanceSensors related bottom:
beacon 9:ac362674c480 8 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 9:ac362674c480 9
beacon 9:ac362674c480 10 AnalogIn sensorVoltage(PB_1);
beacon 9:ac362674c480 11 DigitalOut enable(PC_1);
beacon 9:ac362674c480 12 DigitalOut bit0(PH_1);
beacon 9:ac362674c480 13 DigitalOut bit1(PC_2);
beacon 9:ac362674c480 14 DigitalOut bit2(PC_3);
beacon 9:ac362674c480 15
beacon 9:ac362674c480 16 //DistanceSensors top:
beacon 9:ac362674c480 17 AnalogIn frontS(A1);
beacon 9:ac362674c480 18 AnalogIn leftS(A2);
beacon 9:ac362674c480 19 AnalogIn rightS(A3);
beacon 9:ac362674c480 20
beacon 9:ac362674c480 21 //Leds related:
beacon 9:ac362674c480 22 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 9:ac362674c480 23
beacon 9:ac362674c480 24 //motor related:
beacon 9:ac362674c480 25 PwmOut left(PA_8);
beacon 9:ac362674c480 26 PwmOut right(PA_9);
beacon 9:ac362674c480 27
beacon 9:ac362674c480 28 DigitalOut powerSignal(PB_2);
beacon 9:ac362674c480 29 DigitalIn errorSignal(PB_14);
beacon 9:ac362674c480 30 DigitalIn overtemperatur(PB_15);
beacon 9:ac362674c480 31
beacon 9:ac362674c480 32 //Arm:
beacon 9:ac362674c480 33 Servo servoArm(PA_6);
beacon 9:ac362674c480 34
beacon 9:ac362674c480 35 //Farbsensor:
beacon 9:ac362674c480 36 AnalogIn FarbVoltage(A0);
beacon 9:ac362674c480 37
beacon 9:ac362674c480 38 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm); //Implement the Farbsensor into the Robot init function!!
beacon 9:ac362674c480 39
beacon 9:ac362674c480 40
beacon 9:ac362674c480 41
beacon 9:ac362674c480 42 /* * /
beacon 9:ac362674c480 43 int main(){
beacon 9:ac362674c480 44 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 9:ac362674c480 45 int counter = 0; //Counts how many times the robot has turned, before driving
beacon 9:ac362674c480 46 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 9:ac362674c480 47 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 9:ac362674c480 48 int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 9:ac362674c480 49
beacon 9:ac362674c480 50 while( 1 ){
beacon 9:ac362674c480 51 if ( timer > TIMEOUT ){
beacon 9:ac362674c480 52 NVIC_SystemReset(); //Resets Sam.
beacon 9:ac362674c480 53 }
beacon 9:ac362674c480 54 else if (found == 0){
beacon 9:ac362674c480 55 sam.search(&counter, &timer, &found);
beacon 9:ac362674c480 56 }
beacon 9:ac362674c480 57 else{
beacon 9:ac362674c480 58 //pick up lego
beacon 9:ac362674c480 59 //found = 0;
beacon 9:ac362674c480 60 sam.stop(); //Nur zum Testen
beacon 9:ac362674c480 61 }
beacon 9:ac362674c480 62
beacon 9:ac362674c480 63 wait( 0.1f );
beacon 9:ac362674c480 64 }
beacon 9:ac362674c480 65 return 0;
beacon 9:ac362674c480 66 }
beacon 9:ac362674c480 67
beacon 9:ac362674c480 68 /* * /
beacon 9:ac362674c480 69 int main(){
beacon 9:ac362674c480 70
beacon 9:ac362674c480 71 initializeDistanceSensors();
beacon 9:ac362674c480 72 sam.stop();
beacon 9:ac362674c480 73
beacon 9:ac362674c480 74 while ( 1 ){
beacon 9:ac362674c480 75 for (int i=0; i<6; i++){
beacon 9:ac362674c480 76 sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
beacon 9:ac362674c480 77 }
beacon 9:ac362674c480 78
beacon 9:ac362674c480 79 sam.turnLeftS();
beacon 9:ac362674c480 80
beacon 9:ac362674c480 81 while (sam.sensors[FWD] < NEAR){
beacon 9:ac362674c480 82 wait(0.05f);
beacon 9:ac362674c480 83 sam.stop();
beacon 9:ac362674c480 84 }
beacon 9:ac362674c480 85
beacon 9:ac362674c480 86 }
beacon 9:ac362674c480 87 return 0;
beacon 9:ac362674c480 88 }
beacon 9:ac362674c480 89 /* */
beacon 9:ac362674c480 90
beacon 9:ac362674c480 91 /* * /
beacon 9:ac362674c480 92 int main(){
beacon 9:ac362674c480 93 for(float p=0; p<1.0; p += 0.1) {
beacon 9:ac362674c480 94 // arm = p;
beacon 9:ac362674c480 95 wait(0.2);
beacon 9:ac362674c480 96 }
beacon 9:ac362674c480 97 }
beacon 9:ac362674c480 98 /* */
beacon 9:ac362674c480 99
beacon 9:ac362674c480 100 /* */
beacon 9:ac362674c480 101 int main(){
beacon 9:ac362674c480 102 sam.stop();
beacon 9:ac362674c480 103 int done = 0; //1:= finished process; 0:= not finished
beacon 9:ac362674c480 104 int fun = 0; //just to test.
beacon 9:ac362674c480 105 int start = 0;
beacon 9:ac362674c480 106
beacon 9:ac362674c480 107
beacon 9:ac362674c480 108 while(1){
beacon 9:ac362674c480 109 if(fun == 0){
beacon 9:ac362674c480 110 done = 0;
beacon 9:ac362674c480 111 sam.Arm.collecttoback(&done);
beacon 9:ac362674c480 112 done == 0 ? fun = 0 : fun = 1;
beacon 9:ac362674c480 113 }
beacon 9:ac362674c480 114 else if(fun == 1){
beacon 9:ac362674c480 115 done = 0;
beacon 9:ac362674c480 116 sam.Arm.backtocollect(&done);
beacon 9:ac362674c480 117 done == 0 ? fun = 1 : fun = 2;
beacon 9:ac362674c480 118 }
beacon 9:ac362674c480 119 else if(fun == 2){
beacon 9:ac362674c480 120 done = 0;
beacon 9:ac362674c480 121 sam.Arm.collecttodown(&done);
beacon 9:ac362674c480 122 done == 0 ? fun = 2 : fun = 3;
beacon 9:ac362674c480 123 }
beacon 9:ac362674c480 124 else if(fun == 3){
beacon 9:ac362674c480 125 done = 0;
beacon 9:ac362674c480 126 sam.Arm.downtocollect(&done);
beacon 9:ac362674c480 127 done == 0 ? fun = 3 : fun = 0;
beacon 9:ac362674c480 128 }
beacon 9:ac362674c480 129 wait(0.1);
beacon 9:ac362674c480 130 }
beacon 9:ac362674c480 131 }