zhaw_st16b_pes2_10
k
Dependencies: Servo ServoArm mbed
Revision 0:15a8480061e8, committed 2017-05-22
- Comitter:
- beacon
- Date:
- Mon May 22 11:24:46 2017 +0000
- Commit message:
- o
Changed in this revision
diff -r 000000000000 -r 15a8480061e8 Headers/Declarations.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Headers/Declarations.h Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,45 @@ +#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.12f //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 1400 //Default limit in mV +#define GREEN_DOWNLIMIT 1401// +#define GREEN_UPLIMIT 2000 // + +#define GREEN 1 //Will be used to operate arm functions +#define NOBLOCK 0 // +#define RED 2 // + +//Greifer: +//#define PC_7 SERVO0 + +//Arm +#define COLLECT_POS 60.0f +#define RELEASE_POS 85.0f +#define TAKE_POS -65.0f + + +// +#define UP_POS 20.0f //zu bestimmen +#define DOWN_POS -72.0f //zu bestimmen + +//Misc: +#define TIMEOUT 1400 //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset. +#define MAX 3000 //Once the counter reaches MAX, it will turn around. + +#endif \ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Headers/EncoderCounter.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/EncoderCounter.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+
+ public:
+
+ EncoderCounter(PinName a, PinName b);
+ virtual ~EncoderCounter();
+ void reset();
+ void reset(short offset);
+ short read();
+ operator short();
+
+ private:
+
+ TIM_TypeDef* TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */
diff -r 000000000000 -r 15a8480061e8 Headers/LowpassFilter.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/LowpassFilter.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,39 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+#include <cmath>
+
+/**
+ * This class implements a time-discrete 2nd order low-pass filter for a series of data values.
+ * This filter can typically be used within a periodic task that takes measurements that need
+ * to be filtered, like speed or position values.
+ */
+class LowpassFilter {
+
+ public:
+
+ LowpassFilter();
+ virtual ~LowpassFilter();
+ void reset();
+ void reset(float value);
+ void setPeriod(float period);
+ void setFrequency(float frequency);
+ float getFrequency();
+ float filter(float value);
+
+ private:
+
+ float period;
+ float frequency;
+ float a11, a12, a21, a22, b1, b2;
+ float x1, x2;
+};
+
+#endif /* LOWPASS_FILTER_H_ */
diff -r 000000000000 -r 15a8480061e8 Headers/PID_Control.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/PID_Control.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,55 @@
+/*
+ * PIDControl.h
+ *
+ * Created on: 16.04.2017
+ * Author: chris
+ */
+
+#ifndef COMMON_PID_CONTROL_H_
+#define COMMON_PID_CONTROL_H_
+
+/**
+ * This class calculates a PID control
+ */
+class PID_Control
+{
+public:
+ PID_Control();
+ virtual ~PID_Control();
+
+ float calc(float e, float period);
+ void setPIDValues(float p, float i, float d, float max, float min, float _iMax);
+
+private:
+ /**
+ * the proportional gain
+ */
+ float kp;
+
+ /**
+ * integral gain
+ */
+ float ki;
+
+ /**
+ * differential gain
+ */
+ float kd;
+
+ /**
+ * Sum of all the errors
+ */
+ float iSum;
+
+ /**
+ * Error value one iteration befor
+ */
+ float eOld;
+
+ float max;
+ float min;
+ float iMax;
+
+};
+
+#endif /* COMMON_PID_CONTROL_H_ */
diff -r 000000000000 -r 15a8480061e8 Headers/Pixy.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/Pixy.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,33 @@
+#ifndef PIXY_H
+#define PIXY_H
+#include <mbed.h>
+
+class Pixy
+{
+public:
+ Pixy(Serial& cam);
+
+ struct pixy_s {
+ uint16_t checksum;
+ uint16_t signature;
+ uint16_t x;
+ uint16_t y;
+ uint16_t width;
+ uint16_t height;
+ };
+
+ //returns the X coordinates of the found object in image space
+ int getX();
+ int getY();
+ int getSignature();
+ int getDetects();
+
+private:
+ bool startFound;
+ void rxCallback();
+ int detects;
+ Serial& cam;
+ pixy_s pixy;
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Headers/Robot.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/Robot.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,224 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include <cstdlib>
+#include <mbed.h>
+#include <Ultraschall.h>
+#include "Declarations.h"
+#include "EncoderCounter.h"
+#include "LowpassFilter.h"
+#include "Pixy.h"
+#include "Ultraschall.h"
+#include "Servo.h"
+#include "ServoArm.h"
+#include "Pixy.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(ServoArm* servoArm);
+
+ void init(ServoArm* servoArm);
+
+ int collectToDown();
+ int downToCollect();
+ int collectToBack();
+ int backToCollect();
+ int downToBack();
+ int backToDown();
+
+ private:
+ ServoArm* arm;
+ float angle;
+
+};
+
+class Leiste{
+
+ public:
+
+ Leiste();
+ Leiste(Servo* servoLeiste);
+
+ void init(Servo* servoLeiste);
+
+ int upToDown();
+ int downToUp();
+
+
+ private:
+ Servo* leiste;
+
+
+};
+
+class Greifer
+{
+
+ public:
+
+ Greifer(Servo* greifer);
+ Greifer();
+
+ void init(Servo* greifer);
+ int take();
+ int leave();
+ void nullPos();
+
+ private:
+ Servo* greifer;
+
+};
+
+class USsensor
+{
+ public:
+
+ USsensor();
+ USsensor(Ultraschall* Usensor);
+ void init(Ultraschall* Usensor);
+ float read();
+ operator float();
+
+ private:
+ Ultraschall* Usensor;
+};
+
+class Robot
+{
+
+ public:
+
+ //Robot related:
+ Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor, Pixy* pixy );
+
+ //Drive Functions
+ void drive();
+ void driveB();
+ void setLeft(float pwm);
+ void setRight(float pwm);
+
+ void turnLeft();
+ void turnLeftS();
+ void turnRight();
+ void turnRightS();
+ void turnAround(int left);
+ void stop();
+
+ //Functions that use the drive functions
+ void driveSlowly();
+ void driveBackSlowly();/*
+ 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);
+ */
+ int search(int* timer);
+
+ float see(int sensor);
+ int getErrorMotor();
+
+ //DistanceSensors related:
+ DistanceSensors sensors[9];
+ USsensor USsensor;
+ AnalogIn* frontS;
+ AnalogIn* leftS;
+ AnalogIn* rightS;
+
+
+ void initializeDistanceSensors();
+ //void init();
+
+ //LEDS related:
+ DigitalOut* leds;
+
+ //Farbsensors related:
+ Farbsensor FarbVoltage;
+ DigitalOut* led;
+
+ //Arm related:
+ Arm Arm;
+
+ //Greifer related:
+ Greifer Greifer;
+
+ //Leiste related:
+ Leiste Leiste;
+
+ //Cam:
+ Pixy* pixy;
+
+ private:
+
+ //Robot related:
+ PwmOut* left;
+ PwmOut* right;
+ DigitalIn* errorMotor;
+
+ DigitalOut* powerSignal;
+ DigitalIn* errorSignal;
+ DigitalIn* overtemperatur;
+
+ Servo* greifer;
+ Servo* leiste;
+ ServoArm* arm;
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Headers/Ultraschall.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/Ultraschall.h Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,47 @@
+#ifndef ULTRASCHALL_H
+#define ULTRASCHALL_H
+
+#include "mbed.h"
+
+/** Ultraschall Class(es)
+ */
+
+class Ultraschall
+{
+public:
+ /** Create a Ultraschall object connected to the specified pin
+ * @param pin i/o pin to connect to
+ */
+ Ultraschall();
+ Ultraschall(PinName TrigPin,PinName EchoPin);
+ ~Ultraschall();
+
+ /** Return the distance from obstacle in cm
+ * @param distance in cms and returns -1, in case of failure
+ */
+ unsigned int get_dist_cm(void);
+ /** Return the pulse duration equal to sonic waves travelling to obstacle and back to receiver.
+ * @param pulse duration in microseconds.
+ */
+ unsigned int get_pulse_us(void);
+ /** Generates the trigger pulse of 10us on the trigger PIN.
+ */
+ void start(void);
+ void isr_rise(void);
+ void isr_fall(void);
+ void fall (void (*fptr)(void));
+ void rise (void (*fptr)(void));
+
+
+
+private:
+
+ Timer pulsetime;
+ DigitalOut trigger;
+ InterruptIn echo;
+ unsigned int pulsedur;
+ unsigned int distance;
+};
+
+#endif
+
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 15a8480061e8 ServoArm.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ServoArm.lib Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/zhaw_st16b_pes2_10/code/ServoArm/#fd67069d66e6
diff -r 000000000000 -r 15a8480061e8 Sources/Arm.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Arm.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,95 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+
+Arm::Arm(){
+}
+
+Arm::Arm(ServoArm* arm){
+ init(arm);
+}
+
+void Arm::init(ServoArm* arm){
+ this->arm = arm;
+
+ this->arm->calibrate(0.0015f, 180.0f);
+ this->arm->position(RELEASE_POS);
+}
+
+int Arm::collectToDown(){
+ static float pos=COLLECT_POS;
+ if(pos>TAKE_POS) {
+ pos-=3;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = COLLECT_POS;
+ return 1;
+ }
+}
+
+int Arm::downToCollect(){
+ static float pos=TAKE_POS;
+ if(pos<COLLECT_POS) {
+ pos+=3;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = TAKE_POS;
+ return 1;
+ }
+}
+
+int Arm::collectToBack(){
+ static float pos=COLLECT_POS;
+ if(pos<RELEASE_POS) {
+ pos+=3;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = COLLECT_POS;
+ return 1;
+ }
+}
+
+int Arm::backToCollect(){
+ static float pos=RELEASE_POS;
+ if(pos>COLLECT_POS) {
+ pos-=3;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = RELEASE_POS;
+ return 1;
+ }
+}
+
+int Arm::backToDown(){
+ static float pos = RELEASE_POS;
+ if( pos > TAKE_POS ){
+ pos -= 0.0015f;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = RELEASE_POS;
+ return 1;
+ }
+}
+
+int Arm::downToBack(){
+ static float pos = TAKE_POS;
+ if( pos < RELEASE_POS ){
+ pos += 0.0015f;
+ this->arm->position(pos);
+ return 0;
+ }
+ else{
+ pos = TAKE_POS;
+ return 1;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/DistanceSensors.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/DistanceSensors.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,79 @@
+#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
+ Usensor *= 3.3f;
+ float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628;
+ Distance /= 100;
+
+ static float distance_filtered = 0.0f;
+ distance_filtered = 0.55f * distance_filtered + 0.45f * Distance;
+
+ return Distance;
+ return distance_filtered;
+}
+
+DistanceSensors::operator float()
+{
+ return read();
+}
+
diff -r 000000000000 -r 15a8480061e8 Sources/EncoderCounter.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/EncoderCounter.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,140 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#include "Robot.h"
+#include "Declarations.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+
+ // check pins
+
+ if ((a == PA_6) && (b == PC_7)) {
+
+ // pinmap OK for TIM3 CH1 and CH2
+
+ TIM = TIM3;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // manually enable port C (port A enabled by mbed library)
+
+ // configure general purpose I/O registers
+
+ GPIOA->MODER &= ~GPIO_MODER_MODER6; // reset port A6
+ GPIOA->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port A6
+ GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port A6
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down
+ GPIOA->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port A6
+ GPIOA->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port A6
+
+ GPIOC->MODER &= ~GPIO_MODER_MODER7; // reset port C7
+ GPIOC->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port C7
+ GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port C7
+ GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down
+ GPIOC->AFR[0] &= ~0xF0000000; // reset alternate function of port C7
+ GPIOC->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port C7
+
+ // configure reset and clock control registers
+
+ RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST; //reset TIM3 controller
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // TIM3 clock enable
+
+ } else if ((a == PB_6) && (b == PB_7)) {
+
+ // pinmap OK for TIM4 CH1 and CH2
+
+ TIM = TIM4;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B (port A enabled by mbed library)
+
+ // configure general purpose I/O registers
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER6; // reset port B6
+ GPIOB->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port B6
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port B6
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port B6
+ GPIOB->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port B6
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER7; // reset port B7
+ GPIOB->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port B7
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port B7
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~0xF0000000; // reset alternate function of port B7
+ GPIOB->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port B7
+
+ // configure reset and clock control registers
+
+ RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST; //reset TIM4 controller
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // TIM4 clock enable
+
+ } else {
+
+ printf("pinmap not found for peripheral\n");
+ }
+
+ // configure general purpose timer 3 or 4
+
+ TIM->CR1 = 0x0000; // counter disable
+ TIM->CR2 = 0x0000; // reset master mode selection
+ TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+ TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+ TIM->CCMR2 = 0x0000; // reset capture mode register 2
+ TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+ TIM->CNT = 0x0000; // reset counter value
+ TIM->ARR = 0xFFFF; // auto reload register
+ TIM->CR1 = TIM_CR1_CEN; // counter enable
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+
+ TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(short offset) {
+
+ TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounter::read() {
+
+ return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+
+ return read();
+}
diff -r 000000000000 -r 15a8480061e8 Sources/Farbsensor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Farbsensor.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,53 @@
+#include "Robot.h"
+#include "Declarations.h"
+Serial pc1(USBTX, USBRX);
+
+Farbsensor::Farbsensor()
+{
+}
+
+Farbsensor::Farbsensor(AnalogIn* FarbVoltage)
+{
+ init(FarbVoltage);
+}
+
+void Farbsensor::init(AnalogIn* FarbVoltage)
+{
+ this->FarbVoltage= FarbVoltage;
+}
+
+int Farbsensor::read()
+{
+ static int time=0;
+ static float messungen=0.0f;
+ if( time < 10 ){
+ messungen += FarbVoltage->read();
+ time++;
+ return -1;
+ }
+ else{
+ float Ufarbsensor = messungen/10.0;
+ Ufarbsensor = Ufarbsensor*3300;//Set the Voltage between 0mV und 3300mV
+ pc1.printf("measure = %.0f mV\n", Ufarbsensor);
+
+ messungen = 0.0f;
+ time = 0;
+ if ((Ufarbsensor > GREEN_DOWNLIMIT) && (Ufarbsensor < GREEN_UPLIMIT))
+ {
+ return GREEN;
+ }
+ else if(Ufarbsensor < RED_UPLIMIT)
+ {
+ return RED;
+ }
+ else
+ {
+ return NOBLOCK;
+ }
+ }
+}
+
+Farbsensor::operator int()
+{
+ return read();
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/Greifer.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Greifer.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,58 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+
+
+
+
+
+Greifer::Greifer(Servo* greifer)
+{
+ init(greifer);
+}
+
+Greifer::Greifer()
+{
+
+}
+
+void Greifer::init(Servo* greifer)
+{
+ this->greifer = greifer;
+ greifer->calibrate(0.001f, 90.0f);
+ greifer->position(65.0f);
+}
+
+int Greifer::take()
+
+{
+ this->greifer->position(-70.0f);
+ static int time = 0;
+ if( time < 1000 ){
+ time++;
+ return 0;
+ }
+ else{
+ time = 0;
+ return 1;
+ }
+}
+
+int Greifer::leave()
+{
+ this->greifer->position(65.0f);
+ static int time = 0;
+ if( time < 1000 ){
+ time++;
+ return 0;
+ }
+ else{
+ time = 0;
+ return 1;
+ }
+}
+
+void Greifer::nullPos()
+{
+ this->greifer->position(0.0f);
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/Leiste.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Leiste.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,42 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+
+Leiste::Leiste(){
+}
+
+Leiste::Leiste(Servo* leiste){
+ init(leiste);
+}
+
+void Leiste::init(Servo* leiste){
+ this->leiste = leiste;
+ this->leiste->calibrate(0.0025f, 180.0f);
+ this->leiste->position(UP_POS);
+}
+
+int Leiste::upToDown(){
+ static float pos=UP_POS;
+ if(pos>DOWN_POS) {
+ pos-=3;
+ this->leiste->position(pos);
+ return 0;
+ }
+ else{
+ pos = UP_POS;
+ return 1;
+ }
+}
+
+int Leiste::downToUp(){
+ static float pos=DOWN_POS;
+ if(pos<UP_POS) {
+ pos+=3;
+ this->leiste->position(pos);
+ return 0;
+ }
+ else{
+ pos = DOWN_POS;
+ return 1;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/LowpassFilter.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/LowpassFilter.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,111 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2016, ZHAW
+ * All rights reserved.
+ */
+
+#include "Robot.h"
+#include "Declarations.h"
+
+using namespace std;
+
+/**
+ * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s].
+ */
+LowpassFilter::LowpassFilter() {
+
+ period = 1.0f;
+ frequency = 1000.0f;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+
+ x1 = 0.0f;
+ x2 = 0.0f;
+}
+
+/**
+ * Deletes the LowpassFilter object.
+ */
+LowpassFilter::~LowpassFilter() {}
+
+/**
+ * Resets the filtered value to zero.
+ */
+void LowpassFilter::reset() {
+
+ x1 = 0.0f;
+ x2 = 0.0f;
+}
+
+/**
+ * Resets the filtered value to a given value.
+ * @param value the value to reset the filter to.
+ */
+void LowpassFilter::reset(float value) {
+
+ x1 = value/frequency/frequency;
+ x2 = (x1-a11*x1-b1*value)/a12;
+}
+
+/**
+ * Sets the sampling period of the filter.
+ * This is typically the sampling period of the realtime thread of a controller that uses this filter.
+ * @param the sampling period, given in [s].
+ */
+void LowpassFilter::setPeriod(float period) {
+
+ this->period = period;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Sets the corner frequency of this filter.
+ * @param frequency the corner frequency of the filter in [rad/s].
+ */
+void LowpassFilter::setFrequency(float frequency) {
+
+ this->frequency = frequency;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Gets the current corner frequency of this filter.
+ * @return the current corner frequency in [rad/s].
+ */
+float LowpassFilter::getFrequency() {
+
+ return frequency;
+}
+
+/**
+ * Filters a value.
+ * @param value the original unfiltered value.
+ * @return the filtered value.
+ */
+float LowpassFilter::filter(float value) {
+
+ float x1old = x1;
+ float x2old = x2;
+
+ x1 = a11*x1old+a12*x2old+b1*value;
+ x2 = a21*x1old+a22*x2old+b2*value;
+
+ return frequency*frequency*x1;
+}
diff -r 000000000000 -r 15a8480061e8 Sources/PID_Control.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/PID_Control.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,80 @@
+/*
+ * PIDControl.cpp
+ *
+ * Created on: 16.04.2017
+ * Author: chris
+ */
+
+#include "PID_Control.h"
+#include "Robot.h"
+#include "Declarations.h"
+
+/**
+ * Constructor
+ */
+PID_Control::PID_Control() :
+ kp(0), ki(0), kd(0)
+{
+ eOld = 0.0f;
+ iSum = 0.0f;
+}
+
+/**
+ * Destructor
+ */
+PID_Control::~PID_Control()
+{
+}
+
+/**
+ * Sets the PID values
+ * @param p proportional gain
+ * @param i integral gain
+ * @param d differencial gain
+ */
+void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
+{
+ kp = p;
+ ki = i;
+ kd = d;
+
+ max = _max;
+ min = _min;
+ iMax = _iMax;
+}
+
+/**
+ * Calculate and returns the next value from PID control
+ * @param e the error
+ * @param period the period
+ * @return
+ */
+float PID_Control::calc(float e, float period)
+{
+ static float dpart = 0.0f;
+ float out(0.0f);
+
+ iSum += e;
+
+ //Saturate i part
+ if (iSum > iMax)
+ iSum = iMax;
+ if (iSum < -iMax)
+ iSum = -iMax;
+
+ out = kp * e;
+ out += ki * iSum * period;
+
+ dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;
+ out += kd * dpart;
+
+ // out += kd * (e - eOld) * 1.0f / period;
+
+ eOld = e;
+
+ if( out > max ) out = max;
+ else if( out < min) out = min;
+
+ return out;
+}
+
diff -r 000000000000 -r 15a8480061e8 Sources/Pixy.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Pixy.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,79 @@
+
+#include "Pixy.h"
+#include "Robot.h"
+#include "Declarations.h"
+
+Pixy::Pixy(Serial& _cam) : cam(_cam)
+{
+ //cam.baud( 230400 );
+ cam.baud( 460800 );
+ cam.attach(this, &Pixy::rxCallback, Serial::RxIrq);
+}
+
+
+// This function is called when a character goes into the RX buffer.
+void Pixy::rxCallback()
+{
+ static const int buffersize = 256;
+ this->detects = 0;
+ static int startPoint = 0;
+ static uint8_t buffer[buffersize] = {};
+ static bool startFound = false;
+ static int ii = 1;
+
+ while( cam.readable() ) {
+ buffer[ii] = cam.getc();
+ if( buffer[ii-1] == 85 && (buffer[ii] == 170 ) ) {
+ startPoint = ii-1;
+
+ //check if detection was on the edge of buffer. Skip package if so.
+ if( ii<(buffersize-14))
+ startFound = true;
+ else
+ ii = 1;
+
+ detects++;
+ }
+ ++ii;
+
+ //reset ii
+ if( ii>=(buffersize-1))
+ ii = 1;
+ }
+
+ //start not found, reset ii to 1
+ if( !startFound && ii >= 3 || ii >= (buffersize-1)) {
+ ii = 1;
+ return;
+ }
+
+ //start is found but not enough bytes received - return
+ if( (ii-startPoint) <= 13 )
+ return;
+
+ //copy memory to pixy struct
+ memcpy( &pixy, buffer + startPoint+2, 12);
+
+ //reset variables
+ startFound = false;
+ ii = 1;
+}
+
+int Pixy::getX()
+{
+ return pixy.x;
+}
+
+int Pixy::getY()
+{
+ return pixy.y;
+}
+
+int Pixy::getSignature()
+{
+ return pixy.signature;
+}
+
+int Pixy::getDetects(){
+ return this->detects;
+}
diff -r 000000000000 -r 15a8480061e8 Sources/Robot.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Robot.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,368 @@
+#include "Robot.h"
+#include "Declarations.h"
+
+/* Work in progress -------------------------------------------- */
+
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor, Pixy* pixy)
+{
+ this->left = left;
+ this->right = right;
+ this->powerSignal = powerSignal;
+ //this->errorMotor = errorMotor;
+
+ 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;
+ this->Greifer = Greifer;
+ this->Leiste = Leiste;
+ this->USsensor = USsensor;
+
+ this->pixy = pixy;
+
+}
+
+//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::setLeft(float pwm){
+ *powerSignal = 1;
+ *left = pwm;
+}
+
+void Robot::setRight(float pwm){
+ *powerSignal = 1;
+ *right = pwm;
+}
+
+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;
+}
+
+void Robot::driveSlowly(){
+ static int i = 0;
+ i++;
+ if( i % 2 ){
+ this->drive();
+ }
+ else{
+ this->stop();
+ }
+}
+
+void Robot::driveBackSlowly(){
+ static int i = 0;
+ i++;
+ if( i % 2 ){
+ this->driveB();
+ }
+ else{
+ this->stop();
+ }
+}
+
+/*
+//Functions that use the drive functions
+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::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::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->driveSlowly();
+ }
+ else{
+ this->stop();
+ *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();
+}*/
+
+
+int Robot::search(int* timer){
+ enum states {neutral = 0, counterMax, wallF, wallL, wallR };
+
+ static int state = neutral;
+
+ static int rando = -1;
+
+ static int lastAct = 0;
+
+ int oldx = this->pixy->getX();
+
+ static int counter = 0;
+
+ /*
+ this->sensors[FWD_L] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0;
+ this->sensors[RIGHT_L] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0;
+ this->sensors[LEFT_L] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0;
+ */
+
+
+ //printf("\n\rcurrent robot state: %d", state);
+ if( this->pixy->getDetects() ) return 1;
+ switch( state ){
+ case neutral:
+ if( counter > MAX ){
+ state = counterMax;
+ }
+ else if( this->sensors[FWD].read() < NEAR ){
+ state = wallF;
+ counter = 0;
+ }
+ else if( this->sensors[LEFT].read() < NEAR ){
+ state = wallL;
+ counter = 0;
+ }
+ else if( this->sensors[RIGHT].read() < NEAR ){
+ state = wallR;
+ counter = 0;
+ }
+ else{
+ this->drive();
+ counter = 0;
+ }
+ break;
+
+ case counterMax:{
+ int i = 0;
+ if( i < 1000 ){
+ rando == -1 ? rando = rand() % 2 : rando = rando;
+ this->turnAround(rando);
+ i++;
+ }
+ else{
+ state = neutral;
+ rando = -1;
+ counter = 0;
+ i = 0;
+ }
+ break;
+ }
+
+ case wallF:
+ if( this->sensors[FWD].read() < NEAR ){
+ rando == -1 ? rando = rand() % 2 : rando = rando;
+ this->turnAround(rando);
+ counter++;
+ }
+ else{
+ state = neutral;
+ rando = -1;
+ }
+ break;
+
+ case wallL:
+ if( this->sensors[LEFT].read() < NEAR ){
+ this->turnRight();
+ counter++;
+ }
+ else{
+ state = neutral;
+ }
+ break;
+
+ case wallR:
+ if( this->sensors[RIGHT].read() < NEAR ){
+ this->turnLeft();
+ counter++;
+ }
+ else{
+ state = neutral;
+ }
+ break;
+
+ }
+ return 0;
+}
+
+float Robot::see(int sensor){
+ if( sensor == FWD_L ){
+ return this->USsensor.read();
+ }
+ else{
+ return this->sensors[sensor].read();
+ }
+}
+
+int Robot::getErrorMotor(){
+ return 0; //errorMotor;
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/USsensor.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/USsensor.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,37 @@
+#include <Robot.h>
+
+USsensor::USsensor()
+{
+}
+
+USsensor::USsensor(Ultraschall* Usensor)
+{
+ init(Usensor);
+}
+void USsensor::init(Ultraschall* Usensor)
+{
+ this->Usensor = Usensor;
+}
+
+float USsensor::read()
+{
+ static int i = 0;
+ Usensor->start();
+ if( i ){
+ float dist = Usensor->get_dist_cm();
+ static float distFiltered = dist;
+
+ distFiltered = 0.05f * distFiltered + 0.95f * dist;
+ return distFiltered / 100;
+ //return dist/100.0f;
+ }
+ else{
+ i++;
+ return 0.25f;
+ }
+}
+
+ USsensor::operator float()
+{
+ return read();
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 Sources/Ultraschall.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Ultraschall.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,54 @@
+#include "Ultraschall.h"
+#include "Robot.h"
+#include "Declarations.h"
+
+Ultraschall::Ultraschall(PinName TrigPin,PinName EchoPin):
+ trigger(TrigPin), echo(EchoPin)
+{
+ pulsetime.stop();
+ pulsetime.reset();
+ echo.rise(this,&Ultraschall::isr_rise);
+ echo.fall(this,&Ultraschall::isr_fall);
+ trigger=0;
+}
+
+Ultraschall::~Ultraschall()
+{
+}
+
+void Ultraschall::isr_rise(void)
+{
+ pulsetime.start();
+}
+void Ultraschall::start(void)
+{
+ trigger=1;
+ wait_us(10);
+ trigger=0;
+}
+
+void Ultraschall::isr_fall(void)
+{
+ pulsetime.stop();
+ pulsedur = pulsetime.read_us();
+ distance= (pulsedur*343)/20000;
+ pulsetime.reset();
+}
+
+void Ultraschall::rise (void (*fptr)(void))
+{
+ echo.rise(fptr);
+}
+void Ultraschall::fall (void (*fptr)(void))
+{
+ echo.fall(fptr);
+}
+
+unsigned int Ultraschall::get_dist_cm()
+{
+ return distance;
+}
+unsigned int Ultraschall::get_pulse_us()
+{
+ return pulsedur;
+}
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,341 @@
+#include "mbed.h"
+#include "Robot.h"
+#include "Declarations.h"
+#include "Ultraschall.h"
+#include "Pixy.h"
+#include "PID_Control.h"
+#include "LowpassFilter.h"
+#include "EncoderCounter.h"
+
+#include <cstdlib>
+
+//Cam:
+Serial cam(PA_9, PA_10);
+Pixy pixy(cam);
+
+//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);
+
+
+//Ultraschall
+Ultraschall usensor(D6,D5);
+
+//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 errorMotor(PB_14);
+DigitalIn overtemperatur(PB_15);
+
+//Arm:
+ServoArm servoArm(PA_6);
+
+//Greifer:
+Servo servoGreifer(PC_7);
+
+//Leiste:
+Servo servoLeiste(PB_6);
+
+//Button:
+DigitalIn mybutton(USER_BUTTON);
+
+//Farbsensor:
+AnalogIn FarbVoltage(A0);
+//DigitalOut led(D2);
+
+
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //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.
+ enum states {search = 0, setX, setY, fine, take, test};
+ int time = 0; //Time keeps track of time. [time] = ms
+
+ PID_Control pid;
+ pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
+ pc.baud( 115200 );
+
+ int state = search;
+
+ sam.stop();
+
+ /*
+ while( 1 ){
+ printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
+ wait(1.0f);
+ }
+ */
+
+ while( 1 ){
+ switch( state ){
+ case test:
+ int x = pixy.getX(), y = pixy.getY();
+ if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y);
+ break;
+
+ case search:
+ if( sam.search(&time) ) state = found;
+ break;
+
+ case setX:{
+ static int messungen[20], i = 0;
+ float e = 132.5f - pixy.getX();
+ float diff = pid.calc( e, 0.001f );
+
+ sam.setLeft(0.5f - diff);
+ sam.setRight(0.5f - diff);
+ break;
+ }
+ case setY:{
+ static int messungen[20], i = 0;
+ float e = 121.5f - pixy.getY();
+ float diff = pid.calc( e, 0.001f );
+
+ sam.setLeft(0.5f + diff);
+ sam.setRight(0.5f - diff);
+ break;
+ }
+
+ case take:{
+ //sam.leds[1] = 1;
+ sam.stop();
+ while (1) wait(0.1f);
+ static int down = 0, closed = 0, up = 0;
+ if( down || sam.Arm.backToDown() ) down = 1;
+ else if( closed || sam.Greifer.take() ) closed = 1;
+ else if( up || sam.Arm.downToBack() ) up = 1;
+ else if( sam.Greifer.leave() ){
+ state = search;
+ down = 0;
+ closed = 0;
+ up = 0;
+ }
+ break;
+ }
+ }
+ time++;
+ wait(0.001f);
+ }
+
+ return 0;
+}
+
+/* * /
+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
+ //int done = 0;
+ int color;
+
+ enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
+
+ int state = search;
+
+ static float messung = 0;
+
+ while( 1 ){
+ printf("\n\r%f", sam.see(FWD_L));
+
+ wait(1.0f);
+
+ }
+
+ while( 1 ) {
+
+ if ( timer > TIMEOUT ) {
+ NVIC_SystemReset(); //Resets Sam.
+ }
+
+ //if( timer == 0 )
+ //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
+
+
+ //printf("\n\rcurrent main state: %d", state);
+
+ sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
+ switch( state ) {
+ case search:
+ if( sam.search(&timer) ){
+ //sam.Greifer.nullPos();
+ state = LeisteDown;
+ timer = 0;
+ }
+ break;
+
+ case LeisteDown:
+ int count = 0;
+ if( sam.Leiste.upToDown() ){
+ //sam.Greifer.leave();
+ state = turn;
+ timer = 0;
+ }
+ break;
+
+ case turn:
+ static int i = 0;
+ if( i > 7 ){
+ sam.stop();
+ state = push;
+ i = 0;
+ }
+ else{
+ i++;
+ sam.turnRight();
+ }
+ break;
+
+ case push:{
+ static int i = 0;
+ if( i > 5 ){
+ sam.stop();
+ i = 0;
+ state = backOff;
+ timer = 0;
+ }
+ else{
+ sam.driveSlowly();
+ i++;
+ }
+ break;
+ }
+
+ case backOff:{
+ static int i = 0;
+ if( i > 1 ){
+ sam.stop();
+ i = 0;
+ state = forward;
+ timer = 0;
+ }
+ else{
+ sam.driveBackSlowly();
+ i++;
+ }
+ break;
+ }
+
+
+ case forward:
+ if( sam.Arm.backToCollect() ){
+ state = downward;
+ timer = 0;
+ }
+ break;
+
+ case downward:
+ if( sam.Arm.collectToDown() ){
+ state = down;
+ timer = 0;
+ }
+ break;
+
+ case down:
+ if( sam.Greifer.take() ) {
+ state = upward;
+ timer = 0;
+ }
+ break;
+
+ case upward:
+ if( sam.Arm.downToCollect() ){
+ state = colorS;
+ timer = 0;
+ }
+ break;
+
+ case colorS:
+ led = 1;
+ color = sam.FarbVoltage.read();
+
+ if( color == -1 ){
+ //Do nothing
+ }
+
+
+ else if( color == 0 || color == GREEN ){
+ state = backward;
+ led = 0;
+ timer = 0;
+ }
+
+ else if( color == RED ){
+ state = readyDrop;
+ led = 0;
+ timer = 0;
+ }
+
+ else{
+ //Shit...
+ }
+ break;
+
+ case readyDrop:
+ if( sam.Greifer.leave() ){
+ if( color == GREEN || color == 0 ){
+ state = LeisteUp;
+ }
+ else{
+ state = backward;
+ }
+ timer = 0;
+ }
+
+ break;
+
+ case backward:
+ if( sam.Arm.collectToBack() ){
+ //sam.Greifer.nullPos();
+ state = LeisteUp;
+ timer = 0;
+ if( color == GREEN || color == 0 ){
+ state = readyDrop;
+ sam.Greifer.leave();
+ }
+ else{
+ state = LeisteUp;
+ }
+ }
+ break;
+
+ case LeisteUp:
+ if( sam.Leiste.downToUp() ){
+ state = search;
+ timer = 0;
+ }
+ break;
+ }
+ timer++;
+ wait(0.1f);
+ }
+
+ return 0;
+}
+/* */
\ No newline at end of file
diff -r 000000000000 -r 15a8480061e8 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/4eea097334d6 \ No newline at end of file