Reiko Randoja
/
ut_bbr
Basketball robot mainboard firmware
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