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@10:f76476943a6c, 2017-05-03 (annotated)

Committer:
beacon
Date:
Wed May 03 13:54:51 2017 +0000
Revision:
10:f76476943a6c
Parent:
7:6fed0dcae9c1
Child:
11:292bdbd85a9c
oi

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