Revision 0:88887cfb2b04, committed 2018-09-10

Comitter:

Reiko

Date:

Mon Sep 10 15:24:08 2018 +0000

Commit message:

Mainboard firmware for basketball robot

Changed in this revision

diff -r 000000000000 -r 88887cfb2b04 RFManager/CircularBuffer2.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFManager/CircularBuffer2.h	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,74 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef CIRCULARBUFFER2_H
+#define CIRCULARBUFFER2_H
+
+template <class T>
+class CircularBuffer2 {
+public:
+
+    CircularBuffer2(int length) {
+        write = 0;
+        read = 0;
+        size = length + 1;
+        buf = (T *)malloc(size * sizeof(T));
+    };
+
+    ~CircularBuffer2() {
+        free(buf);
+    }
+
+    bool isFull() {
+        return ((write + 1) % size == read);
+    };
+
+    bool isEmpty() {
+        return (read == write);
+    };
+
+    void queue(T k) {
+        if (isFull()) {
+            read++;
+            read %= size;
+        }
+        buf[write++] = k;
+        write %= size;
+    }
+
+    uint16_t available() {
+        return (write >= read) ? write - read : size - read + write;
+    };
+
+    bool dequeue(T * c) {
+        bool empty = isEmpty();
+        if (!empty) {
+            *c = buf[read++];
+            read %= size;
+        }
+        return(!empty);
+    };
+
+private:
+    volatile uint16_t write;
+    volatile uint16_t read;
+    uint16_t size;
+    T * buf;
+};
+
+#endif

diff -r 000000000000 -r 88887cfb2b04 RFManager/RFManager.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFManager/RFManager.cpp	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,156 @@
+//#include <TARGET_LPC1768/cmsis.h>
+#include "RFManager.h"
+
+RFManager::RFManager(PinName txPinName, PinName rxPinName):
+        serial(txPinName, rxPinName), buf(64) {
+
+    messageAvailable = false;
+    receiveCounter = 0;
+    shortCommandsEnabled = false;
+    shortCommandLength = 5;
+    longCommandLength = 12;
+    commandLength = longCommandLength;
+
+    if (rxPinName == P2_1) {
+        serialId = 1;
+    } else if (rxPinName == P0_11) {
+        serialId = 2;
+    } else if (rxPinName == P0_1) {
+        serialId = 3;
+    } else {
+        serialId = 0;
+    }
+
+    serial.attach(this, &RFManager::rxHandler);
+}
+
+void RFManager::baud(int baudrate) {
+    serial.baud(baudrate);
+}
+
+void RFManager::rxHandler(void) {
+    // Interrupt does not work with RTOS when using standard functions (getc, putc)
+    // https://developer.mbed.org/forum/bugs-suggestions/topic/4217/
+
+    while (serial.readable()) {
+        char c = serialReadChar();
+
+        if (receiveCounter < commandLength) {
+            if (receiveCounter == 0) {
+                // Do not continue before a is received
+                if (c == 'a') {
+                    receiveBuffer[receiveCounter] = c;
+                    receiveCounter++;
+                }
+            } else if (c == 'a' && !shortCommandsEnabled
+                || c == 'a' && shortCommandsEnabled && (receiveCounter < commandLength - 1)
+            ) {
+                // If a is received in the middle, assume some bytes got lost before and start from beginning
+                receiveCounter = 0;
+
+                receiveBuffer[receiveCounter] = c;
+                receiveCounter++;
+            } else {
+                receiveBuffer[receiveCounter] = c;
+                receiveCounter++;
+            }
+
+            if (receiveCounter == commandLength) {
+                receiveCounter = 0;
+
+                for (unsigned int i = 0; i < commandLength; i++) {
+                    buf.queue(receiveBuffer[i]);
+                }
+
+                if (!messageAvailable) {
+                    handleMessage();
+                    //break;
+                }
+            }
+        }
+    }
+}
+
+bool RFManager::readable() {
+    return messageAvailable;
+}
+
+char *RFManager::read() {
+    messageAvailable = false;
+    return receivedMessage;
+}
+
+void RFManager::send(char *sendData) {
+    serialWrite(sendData, commandLength);
+}
+
+void RFManager::send(char *sendData, int length) {
+    serialWrite(sendData, length);
+}
+
+void RFManager::update() {
+    /*if (receiveCounter == commandLength) {
+        handleMessage();
+        _callback.call();
+    }*/
+
+    if (buf.available() >= commandLength) {
+        handleMessage();
+    }
+}
+
+void RFManager::handleMessage() {
+    if (messageAvailable) {
+        return;
+    }
+
+    for (unsigned int i = 0; i < commandLength; i++) {
+        buf.dequeue(receivedMessage + i);
+    }
+
+    receivedMessage[commandLength] = '\0';
+
+    /*receiveCounter = 0;
+
+    memcpy(receivedMessage, receiveBuffer, sizeof(receiveBuffer));*/
+
+    messageAvailable = true;
+}
+
+void RFManager::serialWrite(char *sendData, int length) {
+    int i = 0;
+
+    while (i < length) {
+        if (serial.writeable()) {
+            serial.putc(sendData[i]);
+        }
+        i++;
+    }
+}
+
+char RFManager::serialReadChar() {
+    if (serialId == 1) {
+        return LPC_UART1->RBR;
+    }
+
+    if (serialId == 2) {
+        return LPC_UART2->RBR;
+    }
+
+    if (serialId == 3) {
+        return LPC_UART3->RBR;
+    }
+
+    return LPC_UART0->RBR;
+}
+
+void RFManager::setShortCommandMode(bool isEnabled) {
+    shortCommandsEnabled = isEnabled;
+    receiveCounter = 0;
+
+    if (isEnabled) {
+        commandLength = shortCommandLength;
+    } else {
+        commandLength = longCommandLength;
+    }
+}

diff -r 000000000000 -r 88887cfb2b04 RFManager/RFManager.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RFManager/RFManager.h	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,65 @@
+#ifndef RFMANAGER_H
+#define RFMANAGER_H
+
+#include "mbed.h"
+#include "CircularBuffer2.h"
+
+class RFManager {
+protected:
+    FunctionPointer _callback;
+
+public:
+    RFManager(PinName txPinName, PinName rxPinName);
+
+    void baud(int baudrate);
+
+    char *read();
+
+    void send(char *sendData);
+
+    void send(char *sendData, int length);
+
+    void update();
+
+    void handleMessage();
+
+    void setShortCommandMode(bool isEnabled);
+
+    bool readable();
+
+    void attach(void (*function)(void)) {
+        _callback.attach(function);
+    }
+
+    template<typename T>
+    void attach(T *object, void (T::*member)(void)) {
+        _callback.attach( object, member );
+    }
+
+private:
+    Serial serial;
+
+    int serialId;
+
+    void rxHandler(void);
+
+    bool messageAvailable;
+
+    void serialWrite(char *sendData, int length);
+    char serialReadChar();
+
+    CircularBuffer2<char> buf;
+
+    unsigned int receiveCounter;
+    char receiveBuffer[16];
+
+    char receivedMessage[16];
+
+    bool shortCommandsEnabled;
+    unsigned int commandLength;
+    unsigned int shortCommandLength;
+    unsigned int longCommandLength;
+};
+
+
+#endif //RFManager_H

diff -r 000000000000 -r 88887cfb2b04 RGBLed/RGBLed.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RGBLed/RGBLed.cpp	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,57 @@
+#include "RGBLed.hpp"
+
+RGBLed::RGBLed(PinName rPin, PinName gPin, PinName bPin) :
+    r(rPin),
+    g(gPin),
+    b(bPin)
+{
+    set(false, false, false);
+}
+
+bool RGBLed::getRed()
+{
+    return !r.read();
+}
+
+bool RGBLed::getGreen()
+{
+    return !g.read();
+}
+
+bool RGBLed::getBlue()
+{
+    return !b.read();
+}
+
+RGBLed::Color RGBLed::get()
+{
+    return Color(getRed() | (getGreen() << 1) | (getBlue() << 2));
+}
+
+RGBLed& RGBLed::setRed(bool value)
+{
+    r = !value;
+    return *this;
+}
+
+RGBLed& RGBLed::setGreen(bool value)
+{
+    g = !value;
+    return *this;
+}
+
+RGBLed& RGBLed::setBlue(bool value)
+{
+    b = !value;
+    return *this;
+}
+
+RGBLed& RGBLed::set(bool rValue, bool gValue, bool bValue)
+{
+    return setRed(rValue).setGreen(gValue).setBlue(bValue);
+}
+
+RGBLed& RGBLed::set(RGBLed::Color color)
+{
+    return set(color & 1, color & 2, color & 4);
+}

diff -r 000000000000 -r 88887cfb2b04 RGBLed/RGBLed.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RGBLed/RGBLed.hpp	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,38 @@
+#ifndef RGBLED_H
+#define RGBLED_H
+
+#include <mbed.h>
+
+class RGBLed
+{
+public:
+    enum Color
+    {
+        Black = 0,
+        Red,
+        Green,
+        Yellow,
+        Blue,
+        Magenta,
+        Cyan,
+        White
+    };
+
+    RGBLed(PinName rPin, PinName gPin, PinName bPin);
+
+    bool getRed();
+    bool getGreen();
+    bool getBlue();
+    Color get();
+
+    RGBLed& setRed(bool value);
+    RGBLed& setGreen(bool value);
+    RGBLed& setBlue(bool value);
+    RGBLed& set(bool rValue, bool gValue, bool bValue);
+    RGBLed& set(Color color);
+
+private:
+    DigitalOut r, g, b;
+};
+
+#endif // RGBLED_H

diff -r 000000000000 -r 88887cfb2b04 USBDevice.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice.lib	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/USBDevice/#53949e6131f6

diff -r 000000000000 -r 88887cfb2b04 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+#include "pins.h"
+#include "motor.h"
+#include "RGBLed.hpp"
+#include "USBSerial.h"
+#include "RFManager.h"
+
+USBSerial serial;
+
+Serial pc(USBTX, USBRX);
+
+RGBLed led1(LED1R, LED1G, LED1B);
+RGBLed led2(LED2R, LED2G, LED2B);
+
+DigitalIn infrared(ADC0);
+
+Timeout kicker;
+
+static const int NUMBER_OF_MOTORS = 3;
+
+Motor motor0(&pc, M0_PWM, M0_DIR1, M0_DIR2, M0_FAULT, M0_ENCA, M0_ENCB);
+Motor motor1(&pc, M1_PWM, M1_DIR1, M1_DIR2, M1_FAULT, M1_ENCA, M1_ENCB);
+Motor motor2(&pc, M2_PWM, M2_DIR1, M2_DIR2, M2_FAULT, M2_ENCA, M2_ENCB);
+//Motor motor3(&pc, M3_PWM, M3_DIR1, M3_DIR2, M3_FAULT, M3_ENCA, M3_ENCB);
+
+Motor * motors[NUMBER_OF_MOTORS] = {
+  &motor0, &motor1, &motor2/*, &motor3*/
+};
+
+PwmOut m0(M0_PWM);
+PwmOut m1(M1_PWM);
+PwmOut m2(M2_PWM);
+PwmOut m3(M3_PWM);
+PwmOut pwm0(PWM0);
+PwmOut pwm1(PWM1);
+
+RFManager rfModule(COMTX, COMRX);
+
+void serialInterrupt();
+void parseCommand(char *command);
+
+Ticker pidTicker;
+int pidTickerCount = 0;
+static const float PID_FREQ = 60;
+
+char buf[32];
+int serialCount = 0;
+bool serialData = false;
+
+bool failSafeEnabled = true;
+int ticksSinceCommand = 0;
+
+void pidTick() {
+  for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
+    motors[i]->pidTick();
+  }
+
+  if (pidTickerCount++ % 25 == 0) {
+    led1.setBlue(!led1.getBlue());
+  }
+
+  // Fail-safe
+  if (failSafeEnabled) {
+    ticksSinceCommand++;
+  }
+
+  if (ticksSinceCommand == 60) {
+    for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
+      motors[i]->setSpeed(0);
+    }
+
+    m3.pulsewidth_us(100);
+  }
+}
+
+int main() {
+  pidTicker.attach(pidTick, 1/PID_FREQ);
+  //serial.attach(&serialInterrupt);
+
+  // Ball detector status
+  int infraredStatus = -1;
+
+  // Dribbler motor
+  m3.period_us(20000);
+  m3.pulsewidth_us(100);
+
+  while (1) {
+    if (rfModule.readable()) {
+        serial.printf("<ref:%s>\n", rfModule.read());
+    }
+
+    rfModule.update();
+
+    if (serial.readable()) {
+      buf[serialCount] = serial.getc();
+      //serial.putc(buf[serialCount]);
+
+      if (buf[serialCount] == '\n') {
+        parseCommand(buf);
+        serialCount = 0;
+        memset(buf, 0, 32);
+      } else {
+        serialCount++;
+      }
+    }
+
+    /// INFRARED DETECTOR
+    int newInfraredStatus = infrared.read();
+
+    if (newInfraredStatus != infraredStatus) {
+      infraredStatus = newInfraredStatus;
+      serial.printf("<ball:%d>\n", newInfraredStatus);
+      led2.setGreen(infraredStatus);
+    }
+  }
+}
+
+void parseCommand(char *buffer) {
+  ticksSinceCommand = 0;
+
+  char *cmd = strtok(buffer, ":");
+
+  // buffer == "sd:14:16:10:30"
+  if (strncmp(cmd, "sd", 2) == 0) {
+    for (int i = 0; i < NUMBER_OF_MOTORS; ++i) {
+      motors[i]->setSpeed((int16_t) atoi(strtok(NULL, ":")));
+    }
+
+    /*serial.printf("<gs:%d:%d:%d:%d>\n",
+      motors[0]->getSpeed(),
+      motors[1]->getSpeed(),
+      motors[2]->getSpeed(),
+      motors[3]->getSpeed()
+    );*/
+    
+    serial.printf("<gs:%d:%d:%d>\n",
+      motors[0]->getSpeed(),
+      motors[1]->getSpeed(),
+      motors[2]->getSpeed()
+    );
+  }
+
+  if (strncmp(cmd, "d", 1) == 0) {
+    /*
+    if (command[1] == '0') {
+      pwm1.pulsewidth_us(100);
+    } else if (command[1] == '1') {
+      pwm1.pulsewidth_us(268);
+    } else*/ {
+      m3.pulsewidth_us(atoi(buffer + 2));
+    }
+    //pwm1.pulsewidth_us((int) atoi(command+1));
+    //serial.printf("sending %d\n", (int) atoi(command+1));
+  }
+
+  if (strncmp(cmd, "gs", 2) == 0) {
+    /*serial.printf("<gs:%d:%d:%d:%d>\n",
+      motors[0]->getSpeed(),
+      motors[1]->getSpeed(),
+      motors[2]->getSpeed(),
+      motors[3]->getSpeed()
+    );*/
+    
+    serial.printf("<gs:%d:%d:%d>\n",
+      motors[0]->getSpeed(),
+      motors[1]->getSpeed(),
+      motors[2]->getSpeed()
+    );
+  }
+
+  if (strncmp(cmd, "rf", 2) == 0) {
+       rfModule.send(buffer + 3);
+  }
+
+  if (strncmp(cmd, "r", 1) == 0) {
+    led1.setRed(!led1.getRed());
+  }
+
+  if (strncmp(cmd, "g", 1) == 0) {
+    led1.setGreen(!led1.getGreen());
+  }
+
+  if (strncmp(cmd, "b", 1) == 0) {
+    led1.setBlue(!led1.getBlue());
+  }
+
+  if (strncmp(cmd, "gb", 2) == 0) {
+    serial.printf("<ball:%d>\n", infrared.read());
+  }
+
+  if (strncmp(cmd, "fs", 1) == 0) {
+    failSafeEnabled = buffer[3] != '0';
+  }
+}

diff -r 000000000000 -r 88887cfb2b04 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file

diff -r 000000000000 -r 88887cfb2b04 motor/motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor/motor.cpp	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,151 @@
+#include "motor.h"
+
+Motor::Motor(Serial *pc, PinName pwm, PinName dir1, PinName dir2, PinName fault, PinName encA, PinName encB) :
+    _pwm(pwm),
+    _dir1(dir1),
+    _dir2(dir2),
+    _fault(fault),
+    _encA(encA),
+    _encB(encB),
+    _pc(pc)
+{
+    _encA.mode(PullNone);
+    _encB.mode(PullNone);
+
+    ticks = 0;
+    encNow = 0;
+    encLast = 0;
+
+    enc_last = 0;
+
+    pid_on = 1;
+    currentPWM = 0;
+
+    encTicks = 0;
+    motor_polarity = 0;
+
+    pidSpeed = 0;
+    pidError = 0;
+    pidErrorPrev = 0;
+    pidSetpoint = 0;
+
+    i = 0;
+    counter = 0;
+
+    _pwm.period_us(PWM_PERIOD_US);
+
+    _encA.rise(this, &Motor::encTick);
+    _encA.fall(this, &Motor::encTick);
+    _encB.rise(this, &Motor::encTick);
+    _encB.fall(this, &Motor::encTick);
+
+    //pidTicker.attach(this, &Motor::pidTick, 1/PID_FREQ);
+
+    dir = 0;
+    motor_polarity = 0;
+    pgain = 0.02;
+    igain = 0.005;
+    dgain = 0;
+    pwm_min = 25;
+    intgrl = 0;
+    //count = 0;
+    speed = 0;
+    err = 0;
+}
+
+void Motor::forward(float pwm) {
+    if (dir) {
+        _dir1 = 0;
+        _dir2 = 1;
+    } else {
+        _dir1 = 1;
+        _dir2 = 0;
+    }
+
+    _pwm = pwm;
+    currentPWM = pwm;
+}
+
+void Motor::backward(float pwm) {
+    if (dir) {
+        _dir1 = 1;
+        _dir2 = 0;
+    } else {
+        _dir1 = 0;
+        _dir2 = 1;
+    }
+
+    _pwm = pwm;
+    currentPWM = -pwm;
+}
+
+void Motor::pid2(int16_t encTicks) {
+    speed = encTicks;
+
+    if (!pid_on) return;
+    pidError = pidSetpoint - speed;
+    float p = pgain * pidError;
+    i += igain * pidError;
+    float d = dgain * (pidError - pidErrorPrev);
+    pidSpeed = p + i + d;
+
+    if (i > 1) {
+      i = 1;
+    } else if (i < -1) {
+      i = -1;
+    }
+
+    pidErrorPrev = pidError;
+
+    if (pidSpeed > 1) pidSpeed = 1;
+    if (pidSpeed < -1) pidSpeed = -1;
+
+    if (pidSpeed > 0) forward(pidSpeed);
+    else backward(-pidSpeed);
+
+    if (pidSetpoint == 0) {
+        forward(0);
+        pidError = 0;
+        pidSpeed = 0;
+    }
+}
+
+void Motor::reset_pid() {
+    err = 0;
+    err_prev = 0;
+    i = 0;
+    der = 0;
+    sp = 0;
+    sp_pid = 0;
+    forward(0);
+}
+
+int16_t Motor::getSpeed() {
+    return speed;
+}
+
+void Motor::setSpeed(int16_t speed) {
+    sp_pid = speed;
+    pidSetpoint = speed;
+    if (sp_pid == 0) reset_pid();
+    fail_counter = 0;
+}
+
+void Motor::getPIDGain(char *gain) {
+    sprintf(gain, "PID:%f,%f,%f", pgain, igain, dgain);
+}
+
+void Motor::pidTick() {
+    pid2(ticks);
+    ticks = 0;
+}
+
+void Motor::encTick() {
+    uint8_t enc_dir;
+    encNow = _encA.read() | (_encB.read() << 1);
+    enc_dir = (encLast & 1) ^ ((encNow & 2) >> 1);
+    encLast = encNow;
+
+    if (enc_dir & 1) ticks++;
+    else ticks--;
+}

diff -r 000000000000 -r 88887cfb2b04 motor/motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor/motor.h	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,100 @@
+#ifndef MOTOR_H
+#define  MOTOR_H
+#include "mbed.h"
+#include "PwmOut.h"
+
+class Motor {
+public:
+    //Motor() = default;
+    Motor(Serial *pc, PinName pwm, PinName dir1, PinName dir2, PinName fault, PinName encA, PinName encB);
+    //Motor (const Motor& ) = default;
+
+    uint8_t pid_on;
+    int16_t encTicks;
+    volatile int16_t speed;
+    float pgain, igain, dgain;
+
+    /*
+     * Run this to update speed
+     */
+    void pid2(int16_t encTicks);
+    int16_t getSpeed();
+    void setSpeed (int16_t speed);
+    void getPIDGain(char *gain);
+    void forward(float pwm);
+    void backward(float pwm);
+    void pidTick();
+private:
+    static const unsigned int PWM_PERIOD_US = 2500;
+
+    Serial *_pc;
+    PwmOut _pwm;
+    DigitalOut _dir1;
+    DigitalOut _dir2;
+    DigitalIn _fault;
+    InterruptIn _encA;
+    InterruptIn _encB;
+
+    volatile int16_t ticks;
+    volatile uint8_t encNow;
+    volatile uint8_t encLast;
+    void encTick();
+
+    union doublebyte {
+        unsigned int value;
+        unsigned char bytes[2];
+    };
+
+    uint8_t enc_dir;
+    uint8_t enc_last;
+    uint8_t enc_now;
+
+    uint8_t dir;
+
+    int16_t currentPWM;
+    uint8_t motor_polarity;
+
+    float pidSpeed;
+    float pidError;
+    float pidErrorPrev;
+    float pidSetpoint;
+
+    float i;
+
+/*
+    union doublebyte wcount;
+    union doublebyte decoder_count;
+
+    uint8_t dir;
+*/
+
+    uint8_t fail_counter;
+    uint8_t send_speed;
+    uint8_t failsafe;
+    uint8_t leds_on;
+
+    int16_t sp, sp_pid, der;
+    int16_t intgrl, prop;
+    //int16_t count, speed;
+    int16_t csum;
+    int16_t err, err_prev;
+
+    int16_t pwm, pwm_min, pwmmin;
+    uint8_t update_pid;
+
+    uint16_t stall_counter;
+    uint16_t stallCount;
+    uint16_t prevStallCount;
+    uint16_t stallWarningLimit;
+    uint16_t stallErrorLimit;
+    uint8_t stallLevel;
+    uint8_t stallChanged;
+    //int16_t currentPWM;
+
+    uint8_t counter;
+
+
+    void reset_pid();
+
+};
+#endif //MOTOR_H

diff -r 000000000000 -r 88887cfb2b04 pins.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pins.h	Mon Sep 10 15:24:08 2018 +0000
@@ -0,0 +1,62 @@
+#ifndef PINS_H_
+#define PINS_H_
+#include "mbed.h"
+
+// MOTOR0 (J5)
+#define M0_PWM P2_3
+#define M0_DIR1 P0_21
+#define M0_DIR2 P0_20
+#define M0_ENCA P0_19
+#define M0_ENCB P0_18
+#define M0_FAULT P0_22
+
+// MOTOR1 (J7)
+#define M1_PWM P2_2
+#define M1_DIR1 P0_15
+#define M1_DIR2 P0_16
+#define M1_ENCA P2_7
+#define M1_ENCB P2_6
+#define M1_FAULT P0_17
+
+// MOTOR2 (J8)
+#define M2_PWM P2_1
+#define M2_DIR1 P0_24
+#define M2_DIR2 P0_25
+#define M2_ENCA P0_26
+#define M2_ENCB P0_9
+#define M2_FAULT P0_23
+
+// MOTOR3 (J10)
+#define M3_PWM P2_0
+#define M3_DIR1 P0_7
+#define M3_DIR2 P0_6
+#define M3_ENCA P0_5
+#define M3_ENCB P0_4
+#define M3_FAULT P0_8
+
+// Coilgun
+#define C_CHARGE P0_10
+#define C_KICK P0_11
+#define C_DONE P1_29
+#define PWM0 P2_5
+
+// GPIO0 (J3)
+#define ADC0 P1_30
+#define ADC1 P1_31
+#define PWM1 P2_4
+
+// LED1
+#define LED1R P4_28
+#define LED1G P4_29
+#define LED1B P2_8
+
+// LED2
+#define LED2R P0_28
+#define LED2G P1_18
+#define LED2B P0_27
+
+// COM (J2)
+#define COMTX P0_0
+#define COMRX P0_1
+
+#endif