Revision 0:95384d72794f, committed 2018-05-07

Comitter:

batchee7

Date:

Mon May 07 05:20:37 2018 +0000

Commit message:

IntialRelease

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial/Buffer.lib	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/sam_grove/code/Buffer/#89564915f2a7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial/BufferedSerial.cpp	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,173 @@
+/**
+ * @file    BufferedSerial.cpp
+ * @brief   Software Buffer - Extends mbed Serial functionallity adding irq driven TX and RX
+ * @author  sam grove
+ * @version 1.0
+ * @see
+ *
+ * Copyright (c) 2013
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "BufferedSerial.h"
+#include <stdarg.h>
+
+BufferedSerial::BufferedSerial(PinName tx, PinName rx, uint32_t buf_size, uint32_t tx_multiple, const char* name)
+    : RawSerial(tx, rx) , _rxbuf(buf_size), _txbuf((uint32_t)(tx_multiple*buf_size))
+{
+    RawSerial::attach(this, &BufferedSerial::rxIrq, Serial::RxIrq);
+    this->_buf_size = buf_size;
+    this->_tx_multiple = tx_multiple;   
+    newlines = 0;
+    return;
+}
+
+BufferedSerial::~BufferedSerial(void)
+{
+    RawSerial::attach(NULL, RawSerial::RxIrq);
+    RawSerial::attach(NULL, RawSerial::TxIrq);
+
+    return;
+}
+
+int BufferedSerial::readable(void)
+{
+    return _rxbuf.available();  // note: look if things are in the buffer
+}
+
+int BufferedSerial::writeable(void)
+{
+    return 1;   // buffer allows overwriting by design, always true
+}
+
+int BufferedSerial::getc(void)
+{
+    return _rxbuf;
+}
+
+void BufferedSerial::readLine(char *s)
+{
+    while(! canReadLine());
+    int i = 0;
+    while ((s[i]=(char)getc()) != '\n') {
+        i++;
+    }
+    newlines --;
+    i++;
+    s[i] = '\0';
+}
+
+
+int BufferedSerial::putc(int c)
+{
+    _txbuf = (char)c;
+    BufferedSerial::prime();
+
+    return c;
+}
+
+int BufferedSerial::puts(const char *s)
+{
+    if (s != NULL) {
+        const char* ptr = s;
+    
+        while(*(ptr) != 0) {
+            _txbuf = *(ptr++);
+        }
+        _txbuf = '\n';  // done per puts definition
+        BufferedSerial::prime();
+    
+        return (ptr - s) + 1;
+    }
+    return 0;
+}
+
+int BufferedSerial::printf(const char* format, ...)
+{
+    char buffer[this->_buf_size];
+    memset(buffer,0,this->_buf_size);
+    int r = 0;
+
+    va_list arg;
+    va_start(arg, format);
+    r = vsprintf(buffer, format, arg);
+    // this may not hit the heap but should alert the user anyways
+    if(r > this->_buf_size) {
+        error("%s %d buffer overwrite (max_buf_size: %d exceeded: %d)!\r\n", __FILE__, __LINE__,this->_buf_size,r);
+        va_end(arg);
+        return 0;
+    }
+    va_end(arg);
+    r = BufferedSerial::write(buffer, r);
+
+    return r;
+}
+
+ssize_t BufferedSerial::write(const void *s, size_t length)
+{
+    if (s != NULL && length > 0) {
+        const char* ptr = (const char*)s;
+        const char* end = ptr + length;
+    
+        while (ptr != end) {
+            _txbuf = *(ptr++);
+        }
+        BufferedSerial::prime();
+    
+        return ptr - (const char*)s;
+    }
+    return 0;
+}
+
+
+void BufferedSerial::rxIrq(void)
+{
+    // read from the peripheral and make sure something is available
+    if(serial_readable(&_serial)) {
+        char c = serial_getc(&_serial); // if so load them into a buffer
+        _rxbuf = c;
+        if (c == '\n') newlines ++;
+    }
+    return;
+}
+
+void BufferedSerial::txIrq(void)
+{
+    // see if there is room in the hardware fifo and if something is in the software fifo
+    while(serial_writable(&_serial)) {
+        if(_txbuf.available()) {
+            serial_putc(&_serial, (int)_txbuf.get());
+        } else {
+            // disable the TX interrupt when there is nothing left to send
+            RawSerial::attach(NULL, RawSerial::TxIrq);
+            break;
+        }
+    }
+
+    return;
+}
+
+void BufferedSerial::prime(void)
+{
+    // if already busy then the irq will pick this up
+    if(serial_writable(&_serial)) {
+        RawSerial::attach(NULL, RawSerial::TxIrq);    // make sure not to cause contention in the irq
+        BufferedSerial::txIrq();                // only write to hardware in one place
+        RawSerial::attach(this, &BufferedSerial::txIrq, RawSerial::TxIrq);
+    }
+
+    return;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial/BufferedSerial.h	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,147 @@
+
+/**
+ * @file    BufferedSerial.h
+ * @brief   Software Buffer - Extends mbed Serial functionallity adding irq driven TX and RX
+ * @author  sam grove
+ * @version 1.0
+ * @see     
+ *
+ * Copyright (c) 2013
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef BUFFEREDSERIAL_H
+#define BUFFEREDSERIAL_H
+ 
+#include "mbed.h"
+#include "MyBuffer.h"
+
+/** A serial port (UART) for communication with other serial devices
+ *
+ * Can be used for Full Duplex communication, or Simplex by specifying
+ * one pin as NC (Not Connected)
+ *
+ * Example:
+ * @code
+ *  #include "mbed.h"
+ *  #include "BufferedSerial.h"
+ *
+ *  BufferedSerial pc(USBTX, USBRX);
+ *
+ *  int main()
+ *  { 
+ *      while(1)
+ *      {
+ *          Timer s;
+ *        
+ *          s.start();
+ *          pc.printf("Hello World - buffered\n");
+ *          int buffered_time = s.read_us();
+ *          wait(0.1f); // give time for the buffer to empty
+ *        
+ *          s.reset();
+ *          printf("Hello World - blocking\n");
+ *          int polled_time = s.read_us();
+ *          s.stop();
+ *          wait(0.1f); // give time for the buffer to empty
+ *        
+ *          pc.printf("printf buffered took %d us\n", buffered_time);
+ *          pc.printf("printf blocking took %d us\n", polled_time);
+ *          wait(0.5f);
+ *      }
+ *  }
+ * @endcode
+ */
+
+/**
+ *  @class BufferedSerial
+ *  @brief Software buffers and interrupt driven tx and rx for Serial
+ */  
+class BufferedSerial : public RawSerial 
+{
+private:
+    MyBuffer <char> _rxbuf;
+    MyBuffer <char> _txbuf;
+    uint32_t      _buf_size;
+    uint32_t      _tx_multiple;
+    volatile int newlines; //volatile makes actions atomic, so ++ and -- won't be interrupted.
+    
+    void rxIrq(void);
+    void txIrq(void);
+    void prime(void);
+    
+public:
+    /** Create a BufferedSerial port, connected to the specified transmit and receive pins
+     *  @param tx Transmit pin
+     *  @param rx Receive pin
+     *  @param buf_size printf() buffer size
+     *  @param tx_multiple amount of max printf() present in the internal ring buffer at one time
+     *  @param name optional name
+     *  @note Either tx or rx may be specified as NC if unused
+     */
+    BufferedSerial(PinName tx, PinName rx, uint32_t buf_size = 256, uint32_t tx_multiple = 4,const char* name=NULL);
+    
+    /** Destroy a BufferedSerial port
+     */
+    virtual ~BufferedSerial(void);
+    
+    /** Check on how many bytes are in the rx buffer
+     *  @return 1 if something exists, 0 otherwise
+     */
+    virtual int readable(void);
+    
+    /** Check to see if the tx buffer has room
+     *  @return 1 always has room and can overwrite previous content if too small / slow
+     */
+    virtual int writeable(void);
+    
+    /** Get a single byte from the BufferedSerial Port.
+     *  Should check readable() before calling this.
+     *  @return A byte that came in on the Serial Port
+     */
+    virtual int getc(void);
+    
+    /** Write a single byte to the BufferedSerial Port.
+     *  @param c The byte to write to the Serial Port
+     *  @return The byte that was written to the Serial Port Buffer
+     */
+    virtual int putc(int c);
+    
+    /** Write a string to the BufferedSerial Port. Must be NULL terminated
+     *  @param s The string to write to the Serial Port
+     *  @return The number of bytes written to the Serial Port Buffer
+     */
+    virtual int puts(const char *s);
+    
+    /** Write a formatted string to the BufferedSerial Port.
+     *  @param format The string + format specifiers to write to the Serial Port
+     *  @return The number of bytes written to the Serial Port Buffer
+     */
+    virtual int printf(const char* format, ...);
+    
+    /** Write data to the Buffered Serial Port
+     *  @param s A pointer to data to send
+     *  @param length The amount of data being pointed to
+     *  @return The number of bytes written to the Serial Port Buffer
+     */
+    virtual ssize_t write(const void *s, std::size_t length);
+    
+    bool canReadLine(void) {
+             return newlines;
+             }
+    void readLine(char *s);
+    
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DRV8833.lib	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/batchee7/code/DRV8833/#dc8dc40e1ee7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidController.lib	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/batchee7/code/PidControllerV3/#99403113343f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RGB_LED/RGB_LED.cpp	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,42 @@
+#include "mbed.h"
+#include "RGB_LED.h"
+
+RGB_LED::RGB_LED(PinName RedPin, PinName GreenPin, PinName BluePin):_Rpin(RedPin),
+                                        _Gpin(GreenPin), _Bpin(BluePin)
+{
+    _Rpin = false;
+    _Gpin = false;
+    _Bpin = false;
+}
+
+void RGB_LED::Red()
+{
+    _Rpin = true;
+    _Gpin = false;
+    _Bpin = false;
+    return;
+}
+
+void RGB_LED::Green()
+{
+    _Rpin = false;
+    _Gpin = true;
+    _Bpin = false;
+    return;
+}
+
+void RGB_LED::Blue()
+{
+    _Rpin = false;
+    _Gpin = false;
+    _Bpin = true;
+    return;
+}
+
+void RGB_LED::Off()
+{
+    _Rpin = false;
+    _Gpin = false;
+    _Bpin = false;
+    return;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RGB_LED/RGB_LED.h	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,28 @@
+/*
+    RGB_LED.h - Library for creating Tri Colour LED Objects
+    Created by J. Batchelar, February 15, 2018.
+    Released into the public domain.
+*/
+
+#ifndef RGB_LED_h
+#define RGB_LED_h
+
+#include "mbed.h"
+
+class RGB_LED
+{
+  public:
+    RGB_LED(PinName Redpin, PinName GreenPin, PinName BluePin);
+    void Red();
+    void Green();
+    void Blue();
+    void Off();
+    
+  private:
+    DigitalOut _Rpin;
+    DigitalOut _Gpin;
+    DigitalOut _Bpin;
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,310 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "PidController.h"
+#include "DRV8833.h"
+#include "RGB_LED.h"
+
+#define WHEELCIRC 1.0f              //-- Circumference of drive wheel for speed calc
+#define PULSES_PER_REV 1806.96f     //-- Pulses per revolution for encoder
+
+struct messageToSend {
+    bool Status;
+    bool Position;
+    bool LeftMotorPID;
+    bool RightMotorPID;
+    bool TurningPID;
+    bool WatchDog;
+} ;
+
+
+//#############################################################################
+// -- IO Definitions
+//#############################################################################
+
+DigitalIn mybutton(USER_BUTTON);
+DigitalOut led(LED1);
+
+BufferedSerial gui(PC_10, PC_11);
+//Serial gui(USBTX, USBRX);
+
+
+//#############################################################################
+// -- Class Objects
+//#############################################################################
+
+PidController leftMtrPID;
+PidController rightMtrPID;
+RGB_LED Lights1(PA_5, PA_6, PA_7);
+RGB_LED Lights2(PB_6, PC_7, PA_9);
+DRV8833 LeftMotor(PC_6, PC_8, PC_4, PA_12, PULSES_PER_REV);
+DRV8833 RightMotor(PC_9, PB_8, PC_5, PA_11, PULSES_PER_REV);
+
+Ticker driveTicker;
+
+//#############################################################################
+// -- Global Variables
+//#############################################################################
+char txmsg[80];
+char rxMSG[80];
+char statusMSG[28]; //-- 
+
+
+int msgcounter;     //Used for trigger various message send requests
+messageToSend sendRequests;
+bool EnableDrive;
+bool pidtuneL, pidtuneR;
+float ManualSpeedCmd;
+bool DriveAutomaticMode;
+bool BrakeMotorReq;
+int LeftLastEncCount, RightLastEncCount;    //-- Last recorded encoder count for speed calc
+
+
+//#############################################################################
+// -- Function Prototypes
+//#############################################################################
+void driveSpeedISR(void);
+float CalcDriveSpeed(int currentCount, int lastCount);
+void messageManager(void);
+uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd);
+int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd);
+float ByteArrayToFloat(char *Buffer, uint8_t startAdd);
+
+
+//#############################################################################
+// -- Main Program
+//#############################################################################
+int main()
+{
+
+    gui.baud(115200);           //--ESP Operates at Higher Baud Rate
+    driveTicker.attach(callback(&driveSpeedISR), 0.1);  //-- Call every 100ms
+    
+    led = 1;                    //-- Simple check to see that program downloaded and started correctly
+    
+    //-- Initialise global vars
+    EnableDrive = false;
+    ManualSpeedCmd = 0.0;
+    DriveAutomaticMode = false;
+    BrakeMotorReq = false;
+    pidtuneL = pidtuneR = false;
+    LeftLastEncCount =  RightLastEncCount = 0;
+    
+    gui.printf("Start\n");
+    while(1) {
+        //led = feedback;
+        if (!mybutton) {
+            gui.printf("Bttn\n");
+        }
+        
+        // -- Message Manager to ensure that messages being sent don't overwrite each other
+        messageManager();        
+        
+        // -- enough information for new command
+        if (gui.canReadLine()) {
+            gui.readLine(rxMSG);
+            switch (rxMSG[0]) {
+                case 'W':
+                    sendRequests.WatchDog = true;
+                    break;
+                    
+                case 'C':
+                    EnableDrive = (rxMSG[2] > 0) ? true : false;
+                    ManualSpeedCmd = ByteArrayToFloat(rxMSG, 3);
+                    switch(rxMSG[1])
+                    {
+                        case 'A': DriveAutomaticMode = true;
+                                leftMtrPID.EndDiag();
+                                rightMtrPID.EndDiag();
+                                break;
+                                
+                        case 'M': DriveAutomaticMode = false;
+                                leftMtrPID.EndDiag();
+                                rightMtrPID.EndDiag();
+                                break;
+                                
+                        case 'L': if (!pidtuneL)
+                                {
+                                    leftMtrPID.StartDiag();
+                                    pidtuneL = true;
+                                    }
+                                break;
+                                
+                        case 'R': if (!pidtuneR)
+                                {
+                                    rightMtrPID.StartDiag();
+                                    pidtuneR = true;
+                                    }
+                                break;
+                        }
+                    break;
+
+                case 'L':
+                    leftMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+                    break;
+                    
+                
+                case 'R':
+                    rightMtrPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+                    break;   
+                    
+//                case 'T':
+//                    bearingPID.UpdateSettings(0.0, ByteArrayToFloat(rxMSG, 13), ByteArrayToFloat(rxMSG, 17), ByteArrayToFloat(rxMSG, 21), ByteArrayToFloat(rxMSG, 5), ByteArrayToFloat(rxMSG, 9));
+//                    break;                
+            }
+        }
+    }// -- End of While
+}  //-- End of Main
+
+
+//#############################################################################
+// Attatch this to ticker interupt. Deals with all aspects of the Drive Wheels
+//#############################################################################
+
+void driveSpeedISR(void)
+{
+    float leftSpeed, rightSpeed;
+    int leftPwm, rightPwm;
+    
+    //-- Get Current Speeds 
+    leftSpeed = CalcDriveSpeed(LeftMotor.getCount(), LeftLastEncCount);
+    rightSpeed = CalcDriveSpeed(RightMotor.getCount(), RightLastEncCount);
+    
+    //-- Store Last Count to GLOBAL variabels
+    LeftLastEncCount = LeftMotor.getCount();
+    RightLastEncCount = RightMotor.getCount();
+    
+    
+    
+    //-- Shiloh You will need todo some code here about the AUTO speed
+    //float speedSetpoint;
+   // if (DriveAutomaticMode) speedSetpoint = AutoSpeed;
+    //else speedSetpoint = ManualSpeedCmd;
+    
+    
+    //-- PID 
+    leftPwm = (int)(leftMtrPID.Calculate(ManualSpeedCmd, leftSpeed));
+    rightPwm = (int)(rightMtrPID.Calculate(ManualSpeedCmd, rightSpeed));
+    
+    
+    //-- Outputs to motors
+    if (BrakeMotorReq)
+    {
+        LeftMotor.brake();
+        RightMotor.brake();
+        }
+    else
+    {
+        if (leftPwm > 0) {LeftMotor.forward(leftPwm);}
+        else if (leftPwm < 0) {LeftMotor.reverse(leftPwm);}
+        else LeftMotor.stop();
+    
+        if (rightPwm > 0) {RightMotor.forward(rightPwm);}
+        else if (rightPwm < 0) {RightMotor.reverse(rightPwm);}
+        else RightMotor.stop();
+        }
+    return;
+}
+
+//#############################################################################
+// -- Calculates the Current Wheel Speed (in mm/s)
+//#############################################################################
+float CalcDriveSpeed(int currentCount, int lastCount)
+{
+    float deltaCount = currentCount - lastCount;
+
+    //-- In the event that your just getting flicker send back 0
+    if (abs(deltaCount) < 10) {
+        return 0.0f;
+    } else {
+        //_actualDistance += 109.956f*(deltaCount/PULSES_PER_REV);
+        return WHEELCIRC*10*(deltaCount/PULSES_PER_REV);                            // = Wheel Circ in mm multiply by 10 (as this is called every 100ms)
+    }
+}
+
+//#############################################################################
+// To manage the sending of messages
+//#############################################################################
+void messageManager(void)
+{
+    // **The order that theses IF statements are layed out defines the priority of message
+    if (sendRequests.WatchDog) {
+        gui.printf("W\n");
+        sendRequests.WatchDog = false;
+        return;
+    }
+    
+    if (sendRequests.Status) {
+        gui.printf(statusMSG);
+        sendRequests.Status = false;
+        return;
+        }
+        
+    if (sendRequests.Position) {
+ //       gui.printf("W\n");
+        sendRequests.Position = false;
+        return;
+    }
+    
+    if (sendRequests.LeftMotorPID) {
+        char temp[30];
+        temp[0] = 'L';
+        leftMtrPID.GetDiagnosticsMessage(temp+1);
+        temp[27]='\r';
+        temp[28]='\n';
+        gui.printf(temp);
+        sendRequests.LeftMotorPID = false;
+        return;
+    }
+    
+    if (sendRequests.RightMotorPID) {
+        char temp[30];
+        temp[0] = 'R';
+        rightMtrPID.GetDiagnosticsMessage(temp+1);
+        temp[27]='\r';
+        temp[28]='\n';
+        gui.printf(temp);
+        sendRequests.RightMotorPID = false;
+        return;
+        }
+        
+    if (sendRequests.TurningPID) {
+//        char[30] temp;
+//        temp[0] = "T";
+//        leftMtrPID.GetDiagnosticsMessage(temp+1)
+//        temp[27]='\r';
+//        temp[28]='\n';
+//        gui.printf(temp);
+        sendRequests.TurningPID = false;
+        return;
+    }
+}
+
+//#############################################################################
+// helper functions
+//#############################################################################
+//-- Convert byte array (From C# app) to Unsigned Integer
+uint16_t ByteArrayToUInt(char *Buffer, uint8_t startAdd)
+{
+  uint16_t temp = Buffer[startAdd+1];
+  temp = (temp<<8) | Buffer[startAdd];
+  return temp; 
+  }
+
+//-- Convert byte array (From C# app) to Signed Integer
+int16_t ByteArrayToInt(char *Buffer, uint8_t startAdd)
+{
+  int16_t temp = Buffer[startAdd+1];
+  temp = (temp<<8) | Buffer[startAdd];
+  return temp; 
+  }
+
+//-- Convert byte array (From C# app) to Float
+float ByteArrayToFloat(char *Buffer, uint8_t startAdd)
+{
+  char temp[4]; 
+  temp[0]= Buffer[startAdd];
+  temp[1]= Buffer[startAdd+1];
+  temp[2]= Buffer[startAdd+2];
+  temp[3]= Buffer[startAdd+3];
+  return *((float*)(temp));;
+  }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 07 05:20:37 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file