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 Servo ServoArm
Fork of PES_Yanick by
Revision 8:593f82e66bdf, committed 2017-05-02
- Comitter:
- beacon
- Date:
- Tue May 02 08:39:16 2017 +0000
- Parent:
- 6:ba26dd3251b3
- Child:
- 9:ac362674c480
- Commit message:
- lk;
Changed in this revision
--- 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
