Revision 0:b96618c1411a, committed 2017-05-19

Comitter:

beacon

Date:

Fri May 19 12:09:31 2017 +0000

Commit message:

l

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Control.cpp	Fri May 19 12:09:31 2017 +0000
@@ -0,0 +1,78 @@
+/*
+ * 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;
+    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;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Control.h	Fri May 19 12:09:31 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_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp	Fri May 19 12:09:31 2017 +0000
@@ -0,0 +1,74 @@
+
+#include "Pixy.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;
+    static int 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;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.h	Fri May 19 12:09:31 2017 +0000
@@ -0,0 +1,28 @@
+
+#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();
+
+private:
+    bool startFound;
+    void rxCallback();
+    Serial& cam;
+    pixy_s pixy;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 19 12:09:31 2017 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "Pixy.h"
+#include "PID_Control.h"
+
+//------------------------------------
+// Example for PIxy UART connection.
+// Driver version 0.1
+// BUGS::
+// DO NOT USE PRINTF IN MAIN LOOP FOR PROPER OPERATION
+// Pixy has to be configured for UART with 460800 Baud
+// Pixy connected by UART (PA_9 and PA_10) with 230400 Baud
+//------------------------------------
+
+//communication
+Serial pc(USBTX, USBRX);
+Serial cam(PA_9, PA_10);
+
+DigitalOut myled(LED1);
+
+//motor stuff
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmL(PA_8);
+PwmOut pwmR(PA_9);
+
+Pixy pixy(cam);
+
+int main()
+{
+    PID_Control pid;
+    
+    pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
+    
+    pc.baud( 115200 );
+    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmR.period(0.00005f);
+    enableMotorDriver = 1;
+
+    //int skipper = 0;
+
+    while(1) {
+        wait( 0.005f );
+
+        //  if( ++skipper % 100 == 0)
+        //      printf("x=%d\n\r", pixy.getX());
+
+        float e = 160-pixy.getX();
+        float diff = pid.calc( e, 0.005f );
+        
+        pwmL = 0.5f - diff;
+        pwmR = 0.5f - diff;
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 19 12:09:31 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/4eea097334d6
\ No newline at end of file