Main Code
Dependencies: DRV8833 PidControllerV3 mbed Buffer
Fork of ApexPID by
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
diff -r 000000000000 -r 95384d72794f BufferedSerial/Buffer.lib --- /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
diff -r 000000000000 -r 95384d72794f BufferedSerial/BufferedSerial.cpp --- /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; +} + +
diff -r 000000000000 -r 95384d72794f BufferedSerial/BufferedSerial.h --- /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
diff -r 000000000000 -r 95384d72794f DRV8833.lib --- /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
diff -r 000000000000 -r 95384d72794f PidController.lib --- /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
diff -r 000000000000 -r 95384d72794f RGB_LED/RGB_LED.cpp --- /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
diff -r 000000000000 -r 95384d72794f RGB_LED/RGB_LED.h --- /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
diff -r 000000000000 -r 95384d72794f main.cpp --- /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
diff -r 000000000000 -r 95384d72794f mbed.bld --- /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