to compare with V6, do not change.
Dependencies: mbed
Revision 0:01c109e18d49, committed 2017-05-26
- Comitter:
- obrie829
- Date:
- Fri May 26 12:33:39 2017 +0000
- Commit message:
- RobotV5 in working order
Changed in this revision
diff -r 000000000000 -r 01c109e18d49 Brobot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Brobot.cpp Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,73 @@ +/* + * BROBOT.cpp + * + */ + +#include <cmath> +#include "Brobot.h" + + +Brobot::Brobot(PwmOut* left, PwmOut* right, int number) +{ + pwmL = left; // set local references to objects + pwmR = right; + + this->number = number; +} + +// empty constructor +Brobot::Brobot() +{} + +void Brobot::rotate( float phi) +{ + if(phi>0.5f || phi<-0.5f) { + phi=0; + } + + *pwmL = 0.5f + phi; + *pwmR = 0.5f + phi; +} + +void Brobot::forward() +{ + *pwmL=0.65f; // asterisk is dereferencing the pointer so + *pwmR=0.36f; // you can access the variable at the pointers address + // also another way to dereference the pointer is: pwmR->write(0.xx) +} + +void Brobot::slow(float scale) +{ + if(scale>0.5f || scale<-0.5f) { + scale=0; + } + + *pwmL = *pwmL - scale; + *pwmR = *pwmR + scale; +} + +void Brobot::turnleft() +{ + *pwmL=0.48f; + *pwmR=0.31f; + wait(0.1); +} + +void Brobot::turnright() +{ + *pwmL=0.69f; + *pwmR=0.52f; + wait(0.1); +} + +void Brobot::back() +{ + *pwmR=0.65f; + *pwmL=0.35f; +} + +void Brobot::stop() +{ + *pwmR=0.5f; + *pwmL=0.5f; +} \ No newline at end of file
diff -r 000000000000 -r 01c109e18d49 Brobot.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Brobot.h Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,39 @@ +/* + * Brobot.h + * + */ + +#ifndef BROBOT_H_ +#define BROBOT_H_ + +#include <cstdlib> +#include <mbed.h> + +/** + * This is a device driver class to drive the robot autonomously + */ +class Brobot +{ + +public: + // constructors have same name as the class + Brobot(PwmOut* left, PwmOut* right, int number); + Brobot(); //empty constructor + + + // void init(PwmOut* left, PwmOut* right, int number); + void turnleft(); + void turnright(); + void forward(); + void back(); + void stop(); + void slow(float ammount); // ammount is subtracted from current speeds + void rotate(float ammount); // values from -0.5 to 0.5 + +private: + PwmOut* pwmL; + PwmOut* pwmR; + int number; +}; + +#endif /* BROBOT_H_ */
diff -r 000000000000 -r 01c109e18d49 IRSensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IRSensor.cpp Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,67 @@ +/* + * IRSensor.cpp + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#include <cmath> +#include "IRSensor.h" + + +/** + * Creates an IRSensor object. + * @param distance an analog input object to read the voltage of the sensor. + * @param bit0 a digital output to set the first bit of the multiplexer. + * @param bit1 a digital output to set the second bit of the multiplexer. + * @param bit2 a digital output to set the third bit of the multiplexer. + * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5. + */ +IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) +{ + init(distance, bit0, bit1, bit2, number); +} + + +IRSensor::IRSensor() +{ +} + +void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) +{ + + this->distance = distance; // set local references to objects + this->bit0 = bit0; + this->bit1 = bit1; + this->bit2 = bit2; + + this->number = number; +} + + +/** + * Deletes the IRSensor object. + */ +IRSensor::~IRSensor() {} + +/** + * Gets the distance measured with the IR sensor in [m]. + * @return the distance, given in [m]. + */ +float IRSensor::read() +{ + *bit0 = (number >> 0) & 1; + *bit1 = (number >> 1) & 1; + *bit2 = (number >> 2) & 1; + + float d = -0.58f*sqrt(distance->read())+0.58f; // calculate the distance in [m] + return d; +} + +/** + * The empty operator is a shorthand notation of the <code>read()</code> method. + */ +IRSensor::operator float() +{ + + return read(); +}
diff -r 000000000000 -r 01c109e18d49 IRSensor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IRSensor.h Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,40 @@ +/* + * IRSensor.h + * Copyright (c) 2016, ZHAW + * All rights reserved. + */ + +#ifndef IR_SENSOR_H_ +#define IR_SENSOR_H_ + +#include <cstdlib> +#include <mbed.h> + +/** + * This is a device driver class to read the distance measured with a Sharp IR sensor. + */ +class IRSensor +{ + +public: + // constructors have same name as the class + IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); + IRSensor(); + + void init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number); + virtual ~IRSensor(); // deconstructor + float read(); + + operator float(); + +private: + + AnalogIn* distance; + DigitalOut* bit0; + DigitalOut* bit1; + DigitalOut* bit2; + + int number; +}; + +#endif /* IR_SENSOR_H_ */
diff -r 000000000000 -r 01c109e18d49 PID_Control.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_Control.cpp Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,77 @@ +/* + * PIDControl.cpp + * + * Created on: 16.04.2017 + * Author: chris + */ + +#include "PID_Control.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; //low pass filter + out += kd * dpart; + + // out += kd * (e - eOld) * 1.0f / period; // is affected by noise + + eOld = e; + + if( out > max ) out = max; + else if( out < min) out = min; + + return out; +} +
diff -r 000000000000 -r 01c109e18d49 PID_Control.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_Control.h Fri May 26 12:33:39 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(); // constructor + virtual ~PID_Control(); //destructor + + 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 01c109e18d49 mainV5.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mainV5.cpp Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,126 @@ +#include "mbed.h" +#include "Brobot.h" +#include "IRSensor.h" +#include "PID_Control.h" + +DigitalOut led(LED1); + +//Perophery for distance sensors +AnalogIn distance(PB_1); +DigitalIn button(USER_BUTTON); +DigitalOut enable(PC_1); +DigitalOut bit0(PH_1); +DigitalOut bit1(PC_2); +DigitalOut bit2(PC_3); +IRSensor sensors[6]; + +//indicator leds arround robot +DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; + +//defining the sensors for +IRSensor sensor_front( &distance, &bit0, &bit1, &bit2, 0); +IRSensor sensor_left( &distance, &bit0, &bit1, &bit2, 5); +IRSensor sensor_right( &distance, &bit0, &bit1, &bit2, 1); + +//timer object for ledshow and distance sensor +Ticker t1; + +//motor stuff +DigitalOut enableMotorDriver(PB_2); +PwmOut pwmL(PA_8); +PwmOut pwmR(PA_9); +int number=0; +PID_Control pid; + +Brobot stingray(&pwmL, &pwmR, number); + +//lights up the led according to the sensor signal +void ledDistance() +{ + for( int ii = 0; ii<6; ++ii) + sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0; // an if statement in one line +} + +//blinks at startup and starts ledDistance task +void ledShow() +{ + static int timer = 0; // static is only the first time/loop + for( int ii = 0; ii<6; ++ii) + leds[ii] = !leds[ii]; + + //quit ticker and start led distance show + if( ++timer > 10) { + t1.detach(); + t1.attach( &ledDistance, 0.01f ); + } +} + +//initiate PWM with period and enable +void initPWM() +{ + pwmL.period(0.00005f); + pwmR.period(0.00005f); + pwmL = 0.5f; + pwmR = 0.5f; + enableMotorDriver =1; +} + +// Collision avoidance +void move() +{ + wait(0.01); + float speed = 0.25; + + float e = sensor_left.read() - sensor_right.read(); + float diff = pid.calc( 0.0f+e, 0.005f ); + + pwmL = 0.5f - diff + speed; + pwmR = 0.5f - diff - speed; + + if(sensor_front.read() <=0.3f) { // when approaching normal to wall + stingray.stop(); + int direction=rand()%2 ; // so there is variablility in the robots path + if(direction==0) { + stingray.rotate(0.1f); // and value between 0 and 0.25 + wait(0.5); + } else if (direction==1) { + stingray.rotate(-0.1f); + wait(0.5); + } + } +} + +int main() +{ + enableMotorDriver = 0; //safety precaution + enable = 1 ; //for debugging and output + + t1.attach( &ledShow, 0.05f ); //ticker + + enum robot_states {idle=0, search}; //so that we can reference states by name vs number + int robot_state=idle; // define and start in idle state + + //initialize distance sensors + for( int ii = 0; ii<6; ++ii) + sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii); + + // kp ki kd min max + pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000); + + while(1) { + wait( 0.1f ); //rquired to allow time for ticker + + switch(robot_state) { + case idle: + if(button == 0) { // 0 is pressed + initPWM(); // sets period + robot_state=search; // next state + } + break; + + case search: + move(); + break; + } + } +}
diff -r 000000000000 -r 01c109e18d49 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 26 12:33:39 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/4eea097334d6 \ No newline at end of file