zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Files at this revision

API Documentation at this revision

Comitter:
beacon
Date:
Tue May 02 08:39:16 2017 +0000
Parent:
6:ba26dd3251b3
Child:
9:ac362674c480
Commit message:
lk;

Changed in this revision

Headers/Declarations.h Show diff for this revision Revisions of this file
Headers/Robot.h Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
Sources/Arm.cpp Show diff for this revision Revisions of this file
Sources/DistanceSensors.cpp Show diff for this revision Revisions of this file
Sources/Farbsensor.cpp Show diff for this revision Revisions of this file
Sources/Robot.cpp Show diff for this revision Revisions of this file
Sources/main.cpp Show diff for this revision Revisions of this file
--- a/Headers/Declarations.h	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,40 +0,0 @@
-#ifndef DECLARATIONS_H
-#define DECLARATIONS_H
-
-//DistanceSensors:
-#define NEAR 0.18f          //Used for distance Sensors. If they're to near to a wall -> turn
-#define NEAR_LEGO 0.20f     //If the DistanceSensors are near to a Lego...
-
-#define LEFT_L 5            //Arrayindex of the left LEGO Sensor & left LED
-#define FWD_L 0             //Arrayindex of the front LEGO Sensor & front LED
-#define RIGHT_L 1           //Arrayindex of the right LEGO Sensor & right LED
-
-#define LEFT 7              //Arrayindex of the left Sensor
-#define FWD 6               //Arrayindex of the front Sensor
-#define RIGHT 8             //Arrayindex of the right Sensor
-#define BRIGHT 2            //Arrayindex of the backward right Sensor & left LED
-#define BWD 3               //Arrayindex of the backward Sensor & front LED
-#define BLEFT 4             //Arrayindex of the backsward left Sensor & right LED
-
-//ColorSensors:
-#define RED_UPLIMIT 500     //Default limit in mV
-#define GREEN_DOWNLIMIT 501 //
-#define GREEN_UPLIMIT 1200  //
-
-#define GREEN 1             //Will be used to operate arm functions
-#define NOBLOCK 0           //
-#define RED -1              //
-
-//Greifer:
-#define PC_7 SERVO0
-
-//Arm
-#define COLLECT_POS 65.0f
-#define RELEASE_POS 80.0f
-#define TAKE_POS -70.0f
-
-//Misc:
-#define TIMEOUT 140         //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset.
-#define MAX 50              //Once the counter reaches MAX, it will turn around.
-
-#endif
\ No newline at end of file
--- a/Headers/Robot.h	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,145 +0,0 @@
-#ifndef ROBOT_H
-#define ROBOT_H
-
-#include <cstdlib>
-#include <mbed.h>
-#include "Servo.h"
-
-#include "mbed.h"
-
-
-/* Declarations for the Motors in the Robot in order to drive -------- */
-
-class DistanceSensors
-{
-    public:
-        
-        DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
-        DistanceSensors();
-        
-        void init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
-        virtual ~DistanceSensors();    
-        float read();
-        
-        operator float();           //Is here to constantly return the value of read.
-        
-    private:
-        
-        AnalogIn*       sensorVoltage;
-        AnalogIn*       frontS;
-        AnalogIn*       leftS;
-        AnalogIn*       rightS;
-        
-        DigitalOut*     bit0;
-        DigitalOut*     bit1;
-        DigitalOut*     bit2;
-        
-        int             number;
-
-};
-
-class Farbsensor
-{
-    public:
-     
-        Farbsensor();
-        Farbsensor(AnalogIn* FarbVoltage);
-    
-        void init(AnalogIn* FarbVoltage);
-        int read();
-        operator int();
-    
-    private:
-    
-        AnalogIn*   FarbVoltage;
-};
-
-class Arm{
-    
-    public:
-        
-        Arm();
-        Arm(Servo* servoArm);
-        
-        void init(Servo* servoArm);
-        
-        void collecttodown(int* done);
-        void downtocollect(int* done);
-        void collecttoback(int* done);
-        void backtocollect(int* done);
-        
-    private:
-        Servo*  servoArm;
-        float   angle;
-        
-};
-
-class Robot
-{
-    
-    public:
-        
-        //Robot related:
-        Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage,  AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, Servo* Arm );
-        
-        //Drive Functions
-        void drive();
-        void driveB();
-
-        void turnLeft();
-        void turnLeftS();
-        void turnRight();
-        void turnRightS();
-        void turnAround(int left);
-        void stop();
-        
-        //Functions that use the drive functions
-        void wallRight(int* counter, int* timer, int* lastAct);
-        void wallLeft(int* counter, int* timer, int* lastAct);
-        void wallFront(int* counter, int* timer, int* lastAct);
-        void counterMax(int* counter, int* timer, int* lastAct, int* rando);
-        
-        void legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found);
-        void legoRight(int* counter, int* timer, int* lastAct, int* legoFound);
-        void legoLeft(int* counter, int* timer, int* lastAct, int* legoFound);
-        
-        void nothingFound(int* counter, int* timer, int* lastAct);
-        
-        void search(int* counter, int* timer, int* found);
-        void lego(int* counter, Timer* t);
-        
-        //DistanceSensors related:
-        DistanceSensors sensors[9];
-        AnalogIn* frontS;
-        AnalogIn* leftS;
-        AnalogIn* rightS;
-        
-        
-        void initializeDistanceSensors();
-        //void init();
-        
-        //LEDS related:
-        DigitalOut*     leds;
-        
-        //Farbsensors related:
-        Farbsensor      FarbVoltage;
-        
-        //Arm related:
-        Arm             Arm;
-        
-    private:
-    
-        //Robot related:
-        PwmOut*         left;
-        PwmOut*         right;
-    
-        DigitalOut*     powerSignal;
-        DigitalIn*      errorSignal;
-        DigitalIn*      overtemperatur;
-        
-        Servo*          arm;
-        
-
-};
-
-#endif
\ No newline at end of file
--- a/Servo.lib	Wed Apr 26 14:09:08 2017 +0000
+++ b/Servo.lib	Tue May 02 08:39:16 2017 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07
+https://developer.mbed.org/teams/zhaw_st16b_pes2_10/code/Servo/#92981b40ceb1
--- a/Sources/Arm.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,61 +0,0 @@
-#include "Robot.h"
-#include "Declarations.h"
-
-
-Arm::Arm(){
-}
-
-Arm::Arm(Servo* servoArm){
-    init(servoArm);
-}
-
-void Arm::init(Servo* servoArm){
-    this->servoArm = servoArm;
-    
-    this->servoArm->calibrate(0.0015f, 180.0f);
-    this->servoArm->position(COLLECT_POS);
-}
-
-void Arm::collecttodown(int* done){
-    static float pos=COLLECT_POS;
-    if(pos>TAKE_POS) {
-        pos-=0.5f;
-        this->servoArm->position(pos);
-    }
-    else{
-        *done = 1;
-    }
-}
-
-void Arm::downtocollect(int* done){
-    static float pos=TAKE_POS;
-    if(pos<COLLECT_POS) {
-        pos+=0.5f;
-        this->servoArm->position(pos);
-        }
-    else{
-        *done = 1;
-    }
-}
-
-void Arm::collecttoback(int* done){
-    static float pos=COLLECT_POS;
-    if(pos<RELEASE_POS) {
-        pos+=0.5f;
-        this->servoArm->position(pos);
-        }
-    else{
-        *done = 1;
-    }
-}
-
-void Arm::backtocollect(int* done){
-    static float pos=RELEASE_POS;
-    if(pos>COLLECT_POS) {
-        pos-=0.5f;
-        this->servoArm->position(pos);
-        }
-    else{
-        *done = 1;
-    }
-}
\ No newline at end of file
--- a/Sources/DistanceSensors.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,72 +0,0 @@
-#include <cmath>
-#include "Robot.h"
-#include "Declarations.h"
-
-
-
-DistanceSensors::DistanceSensors() 
-{
-    //Nothing
-}
-    
-DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
-{   
-    init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
-}
-
-//initialise
-void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
-{
-    this->sensorVoltage = sensorVoltage;
-    this->frontS = frontS;
-    this->leftS = leftS;
-    this->rightS = rightS;
-    
-    this->bit0 = bit0;
-    this->bit1 = bit1;
-    this->bit2 = bit2;
-    this->number = number;
-}
-
-
-DistanceSensors::~DistanceSensors()
-{ 
-}
-
-
-float DistanceSensors::read()//Return the distance of an object
-{
-    
-    float Usensor;
-  
-    if (number < 6){
-        *bit0 = number & 1; // Set the first bit of the Sensors MUX
-        *bit1 = number & 2; // Set the second bit of the Sensors MUX
-        *bit2 = number & 4; // Set the third bit of the Sensors MUX
-        
-        Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
-    }
-    else{
-        switch(number){
-            case 6:
-                Usensor=frontS->read();
-                break;
-            case 7:
-                Usensor=leftS->read();
-                break;
-            case 8:
-                Usensor=rightS->read();
-                break;
-        }
-    }
-    
-    //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
-    float Distance=-0.38f*sqrt(Usensor)+0.38f;
-    return Distance;
-}
-
-DistanceSensors::operator float()
-{ 
-    return read();
-}
-
--- a/Sources/Farbsensor.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,43 +0,0 @@
-#include "Robot.h"
-#include "Declarations.h"
-
-
-Farbsensor::Farbsensor()
-{   
-}
-
-Farbsensor::Farbsensor(AnalogIn* FarbVoltage)
-{
-    init(FarbVoltage);  
-}
-
-void Farbsensor::init(AnalogIn* FarbVoltage)
-{
-    this->FarbVoltage= FarbVoltage;
-}
-
-int Farbsensor::read()
-{
-    int farbe;  
-    float Ufarbsensor=FarbVoltage->read();
-    Ufarbsensor=Ufarbsensor*3300; //Set the Voltage between 0mV und 3300mV
-
-    if ((Ufarbsensor > GREEN_DOWNLIMIT) && (Ufarbsensor < GREEN_UPLIMIT))
-        {
-         farbe=1;
-        }
-    else if(Ufarbsensor < RED_UPLIMIT)
-        {
-         farbe=-1;
-        }
-    else 
-        {
-         farbe=0;
-        }
-    return farbe;
-}
-
-Farbsensor::operator int()
-{
-    return read();
-}
\ No newline at end of file
--- a/Sources/Robot.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,348 +0,0 @@
-#include "Robot.h"
-#include "Declarations.h"
-
-/* Work in progress -------------------------------------------- */
-
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, Servo* Arm )
-{
-    this->powerSignal = enableSignal;
-    this->left =        left;
-    this->right =       right;
-
-    this->left->period(0.00005f);
-    this->right->period(0.00005f);
-    
-    this->leds =        leds;
-    
-    this->FarbVoltage = FarbVoltage;
-    this->frontS =      frontS;
-    this->leftS =       leftS;
-    this->rightS =      rightS;
-    
-    this->Arm =         Arm;
-
-}
-
-//Drive functions
-void Robot::drive()
-{
-    //pwm determine what direction it goes.
-    *powerSignal = 1;
-    *left=  0.6f;
-    *right= 0.4f;
-}
-
-void Robot::driveB()
-{
-    //pwm determine what direction it goes.
-    *powerSignal = 1;
-    *left=  0.4f;
-    *right= 0.6f;
-}
-
-void Robot::turnLeft()
-{
-    
-    *powerSignal = 1;
-    *left=  0.4f;
-    *right= 0.4f;
-
-}
-
-void Robot::turnLeftS()
-{
-    
-    *powerSignal = 1;
-    *left=  0.45f;
-    *right= 0.45f;
-
-}
-
-void Robot::turnRight()
-{
-    *powerSignal = 1;
-    *left=  0.6f;
-    *right= 0.6f;
-}
-
-void Robot::turnRightS()
-{
-    
-    *powerSignal = 1;
-    *left=  0.55f;
-    *right= 0.55f;
-
-}
-
-void Robot::turnAround(int left)
-{
-    *powerSignal = 1;
-
-    if (left) {
-        turnLeft();
-    }
-
-    else {
-        turnRight();
-    }
-}
-
-void Robot::stop()
-{
-    *powerSignal = 1;
-    *left=  0.5f;
-    *right= 0.5f;
-}
-
-
-//Functions that use the drive functions
-void Robot::wallRight(int* counter, int* timer, int* lastAct){
-    *counter += 1;
-        
-    if (*lastAct != 1){             //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 1;
-    }
-        
-    this->turnLeft();
-}
-
-void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
-    if (*lastAct != 0){                 //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 0;
-    }
-    
-    if (*rando == -1){                  //If rando was unused, set a new number.
-        *rando = rand() % 2;
-    }
-    
-    if (this->sensors[FWD] < NEAR){   //While something is seen turn around.
-        this->turnAround(*rando);
-    }
-    
-    else{
-        *rando = -1;
-        *counter = 0;
-    }
-}
-
-void Robot::wallLeft(int* counter, int* timer, int* lastAct){
-    *counter += 1;
-        
-    if (*lastAct != 2){             //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 2;
-    }
-        
-    this->turnRight();
-}
-
-void Robot::wallFront(int* counter, int* timer, int* lastAct){
-    if (*lastAct != 3){              //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 3;
-    }
-        
-    *counter = MAX;                   //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
-}
-
-
-void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){
-    //*counter += 1;
-    *legoFound = 0;
-    
-    if (*lastAct != 4){                 //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 4;
-    }
-    
-    if (this->sensors[FWD] < NEAR){     //If Sam sees a wall turn around
-        *legoFound = -1;
-        *counter = MAX;                 //setting counter to MAX will couse sam to turnAround
-    }
-    
-    if (this->sensors[FWD_L] > 0.16f){
-        this->drive();
-    }
-    else{
-        *found = 1;
-    }
-}
-
-void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
-    //*counter += 1;
-    *legoFound = 1;
-    
-    if (*lastAct != 5){          //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 5;
-    }
-    
-    if (this->sensors[FWD_L] > 0.22f){
-        this->turnRight();
-    }
-    else{
-        this->stop();
-        *legoFound = -1;
-    }
-}
-
-void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
-    //*counter += 1;
-    *legoFound = 2;
-    
-    if (*lastAct != 6){          //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 6;
-    }
-    
-    if (this->sensors[FWD_L] > 0.22f){
-        this->turnLeft();
-    }
-    else{
-        this->stop();
-        *legoFound = -1;
-    }
-}
-
-
-void Robot::nothingFound(int* counter, int* timer, int* lastAct){
-    *counter = 0;    
-    if (*lastAct != 7){          //If this wasn't the last called action, reset the timer.
-        *timer = 0;
-        *lastAct = 7;
-    }
-    
-    this->drive();
-}
-
-
-void Robot::search(int* counter, int* timer, int* found){
-    
-    static int rando = -1;          //Rando will be used, to randomize turnAround()
-                                    //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
-    
-    static int lastAct = -1;        //Is used, to check if the same action in Robot::search() was made multiple times.
-                                    //-1 := unused; x != -1 := action x was called last.
-                                    
-    static int legoFound = -1;      //Is used to determine, on what side the lego was found.
-                                    //-1 := unused; 0:= front; 1:= right, 2:= left.
-                                    
-    *timer += 1;                    //timer holds the time in 0.1s
-    
-    
-    if (*counter >= MAX) {          //Robot is stuck turning left & right
-        counterMax(counter, timer, &lastAct, &rando);
-    }
-    
-    /*//Wall actions:
-    else if (this->sensors[RIGHT] < NEAR && legoFound == -1){   //Robot has spotted an obstacle on the right.
-        wallRight(counter, timer, &lastAct);
-    }
-    */
-    else if (this->sensors[LEFT] < NEAR && legoFound == -1) {   //Robot has spotted an obstacle on the left.
-        wallLeft(counter, timer, &lastAct);
-    }
-    
-    else if (this->sensors[FWD] < NEAR && legoFound == -1)  {   //Robot has spotted an obstacle infront
-        wallFront(counter, timer, &lastAct);
-    }
-    
-    //Lego actions:
-    else if (this->sensors[FWD_L] < NEAR_LEGO && legoFound == -1 || legoFound == 0){    //There's a Lego in front.
-        legoFront(counter, timer, &lastAct, &legoFound, found);
-    }
-
-    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){  //There's a Lego on the right.
-        legoRight(counter, timer, &lastAct, &legoFound);
-    }
-    
-    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){   //There's a Lego on the left.
-        legoLeft(counter, timer, &lastAct, &legoFound);
-    }
-    
-    //Lego is ready to be grabbed.
-    else if(legoFound == 3){
-        *found = 1;             //When found is set to 1, main will call arms functions
-        *counter = 0;
-        *timer = 0;
-        legoFound = -1;
-        
-        //Old! Check if still useful!{
-            //check if this was the last action
-                //reset timer
-    
-            //dont add up counter
-            //use arm
-            //set legoFound to -1
-        //}
-    }
-    
-    
-    //Nothing found
-    else {
-        nothingFound(counter, timer, &lastAct);
-    }
-}
-
-void Robot::lego(int* counter, Timer* t){
-    
-    if (this->sensors[RIGHT_L] < NEAR_LEGO){
-        t->reset();
-        
-        *counter += 1;
-        while (this->sensors[FWD] > NEAR_LEGO){
-            if ( t->read() > TIMEOUT ){
-                break;
-            }
-            
-            this->turnRight();
-        }
-        this->stop();
-    }
-    
-    else if (this->sensors[LEFT] < NEAR_LEGO){
-        t-> reset();
-        
-        *counter += 1;
-        while (this->sensors[FWD] > NEAR_LEGO){
-            if ( t->read() > TIMEOUT ){
-                break;
-            }
-            
-            this->turnLeft();
-        }
-        this->stop();
-    }
-    
-    else if (this->sensors[FWD] < 0.12f){
-        while (this->sensors[FWD] < 0.12f){
-            this->driveB();
-        }
-        while (this->sensors[FWD] < NEAR_LEGO){
-            this->turnRight();
-        }
-        while(1){
-            this->stop();
-        }
-    }
-    
-    else {
-        *counter = 0;
-        this->drive();
-    }
-}
-
-//void Robot::init(){
-//  Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
-//}
-
-//Remember to set
-//DigitalOut powerMotor(PB_2) = 1;
-//DigitalIn errorMotor(PB_14);
-//if (errorMotor){
-//reset
-//}
-
-//Add management for Overpower!! Pin PB_15
\ No newline at end of file
--- a/Sources/main.cpp	Wed Apr 26 14:09:08 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,135 +0,0 @@
-#include "mbed.h"
-#include "Robot.h"
-#include "Declarations.h"
-
-#include <cstdlib>
-
-//DistanceSensors related bottom:
-Serial pc(SERIAL_TX, SERIAL_RX);
-
-AnalogIn sensorVoltage(PB_1);
-DigitalOut enable(PC_1);
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
-
-//DistanceSensors top:
-AnalogIn frontS(A1);
-AnalogIn leftS(A2);
-AnalogIn rightS(A3);
-
-//Leds related:
-DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
-
-//motor related:
-PwmOut          left(PA_8);
-PwmOut          right(PA_9);
-
-DigitalOut      powerSignal(PB_2);
-DigitalIn       errorSignal(PB_14);
-DigitalIn       overtemperatur(PB_15);
-
-//Arm:
-Servo servoArm(PA_6);
-
-//Farbsensor:
-AnalogIn FarbVoltage(A0);
-
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm); //Implement the Farbsensor into the Robot init function!!
-
-void initializeDistanceSensors(){
-    for( int ii = 0; ii<9; ++ii) {
-        sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
-
-    enable = 1;
-    }
-}
-
-/* * /
-int main(){
-    initializeDistanceSensors();        //Initialises IR-Sensors.
-    int counter = 0;                    //Counts how many times the robot has turned, before driving
-    int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
-    //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
-    int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
-    
-    while( 1 ){
-        if ( timer > TIMEOUT ){
-            NVIC_SystemReset();         //Resets Sam.
-        }
-        else if (found == 0){
-            sam.search(&counter, &timer, &found);
-        }
-        else{
-            //pick up lego
-            //found = 0;
-            sam.stop(); //Nur zum Testen
-        }
-        
-        wait( 0.1f );
-    }
-    return 0;
-}
-
-/* * /
-int main(){
-    
-    initializeDistanceSensors();
-    sam.stop();
-    
-    while ( 1 ){
-        for (int i=0; i<6; i++){
-            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
-        }
-        
-        sam.turnLeftS();
-        
-        while (sam.sensors[FWD] < NEAR){
-            wait(0.05f);
-            sam.stop();
-        }
-        
-    }
-    return 0;
-}
-/* */
-
-/* * /
-int main(){
-    for(float p=0; p<1.0; p += 0.1) {
-      //  arm = p;
-        wait(0.2);
-    }
-}
-/* */
-
-/* */
-int main(){
-    sam.stop();
-    int done = 0;           //1:= finished process; 0:= not finished
-    int fun = 0;            //just to test.
-    
-    while(1){
-        if(fun == 0){
-            done = 0;
-            sam.Arm.collecttoback(&done);
-            done == 0 ? fun = 0 : fun = 1;
-        }
-        else if(fun == 1){
-            done = 0;
-            sam.Arm.backtocollect(&done);
-            done == 0 ? fun = 1 : fun = 2;
-        }
-        else if(fun == 2){
-            done = 0;
-            sam.Arm.collecttodown(&done);
-            done == 0 ? fun = 2 : fun = 3;
-        }
-        else if(fun == 3){
-            done = 0;
-            sam.Arm.downtocollect(&done);
-            done == 0 ? fun = 3 : fun = 0;
-        }
-        wait(0.1);
-    }
-}    
\ No newline at end of file