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

--- /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

--- /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_ */

--- /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();
+}

--- /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_ */

--- /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;
+}
+

--- /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_ */

--- /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;
+        }
+    }
+}

--- /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