Revision 0:214d306295fa, committed 2017-05-30

Comitter:

huynh270

Date:

Tue May 30 09:56:04 2017 +0000

Commit message:

hello

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Control.cpp	Tue May 30 09:56:04 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;
+    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;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Control.h	Tue May 30 09:56:04 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_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp	Tue May 30 09:56:04 2017 +0000
@@ -0,0 +1,109 @@
+#include "Pixy.h"
+
+Pixy::Pixy(Serial& _cam) : cam(_cam), detects(0)
+{
+    //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 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;
+}
+
+/**
+returns the width of the detected object
+**/
+int Pixy::getWidth(  )
+{
+    return pixy.width;
+}
+
+/**
+returns the height of the detected object
+**/
+int Pixy::getHeight(  )
+{
+    return pixy.height;
+}
+
+/**
+returns the Y coordinates in respect to the pixy recorded image
+**/
+int Pixy::getY( )
+{
+    return pixy.y;
+}
+
+/**
+returns the Y coordinates in respect to the pixy recorded image
+**/
+int Pixy::getX(  )
+{
+    return pixy.x;
+}
+
+
+/**
+returns the signature of the detected object
+**/
+int Pixy::getSignature()
+{
+    return pixy.signature;
+}
+
+/**
+returns 1 if an object has detectd
+**/
+bool Pixy::objectDetected()
+{
+    bool ret = false;
+    static int oldDetection = 0;
+    ret = (detects != oldDetection);
+    oldDetection = detects;
+    return ret;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.h	Tue May 30 09:56:04 2017 +0000
@@ -0,0 +1,32 @@
+#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;
+    };
+ 
+    int getX();
+    int getY();
+    int getSignature();
+    int getHeight();
+    int getWidth();
+    bool objectDetected();
+ 
+private:
+    void rxCallback();
+     
+    bool startFound;
+    int detects;
+     
+    Serial& cam;
+    pixy_s pixy;
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 30 09:56:04 2017 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "Pixy.h"
+#include "PID_Control.h"
+ 
+//------------------------------------
+// Example for PIxy UART connection.
+// Driver version 0.2
+// BUGS::
+// DO NOT USE PRINTF IN MAIN LOOP FOR PROPER OPERATION
+// Pixy connected by UART (PA_9 and PA_10) with 460800 Baud
+// Sample tries to center the recognised object between the pixy cam
+//------------------------------------
+ 
+//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.01f, 0.0000f, 0.000000f, 0.3f, -0.3f, 1000);
+ 
+    pc.baud( 115200 );
+    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmR.period(0.00005f);
+ 
+    //Wait untill pixy is ready
+    wait( 2.0f);
+ 
+    //enable motors
+    enableMotorDriver = 1;
+ 
+    //start while loop
+    while(1) {
+        wait( 0.005f );
+        if( pixy.objectDetected() && pixy.getSignature() == 1) {
+            float e = 160-pixy.getX();
+            float diff = pid.calc( e, 0.005f );
+            pwmL = 0.5f - diff;
+            pwmR = 0.5f - diff;
+        } else {
+        pwmL = 0.5f ;
+        pwmR = 0.5f;
+        }
+    }
+}

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