From 02d4d0029586ddbfb50349a214a0c1aa25dce662 Mon Sep 17 00:00:00 2001 From: SilentCtrl Date: Wed, 27 Feb 2019 20:57:25 -0800 Subject: [PATCH] Polar Bear Controller Code (#667) * Use profiling and event loop libs * [HIBIKE] C extension (#622) * Add hibike_packet as submodule * Enable use of hibike_packet Detects whether the extension is installed by trying to import it. * Update hibike_packet * Remove process_buffer * [HIBIKE] fixing Disconnect and register_sensor syntax errors * Final preparation for asyncio merge (#631) * Do not crash on invalid COBS data * Rename hibike_process_async to hibike_process Remove old hibike_process and replace it with async version. API compatibility is maintained, so Runtime will not be changed. * Prevent hibike_tester hang after termination hibike_tester would hang after terminating hibike_process, because one of its threads was still running. We tell the thread to shut down after the process is done instead of running forever. * Remove stub file * Remove send_transport This function is identical to send, so it doesn't make sense to keep it around. * Replace virtual device with async version The asynchronous version uses less CPU and has saner defaults. * Remove virtual device dependency on hibike_process Some parts of the virtual device perform similar functions to parts of hibike_process, but it is better that the two implementations be allowed to evolve independently. * Update tests for async; add read/write tests Async tests need to deal with the event loop. In addition, we test that hibike is actually reading and writing to devices using virtual devices. * Remove outdated portions of README, update others * Add explanation for read/write retries * Add test for nonexistent device errors As it turns out, we were not sending error messages when a nonexistent device was accessed; a test should ensure this behavior stays in. * Update developer documentation * Fix lint errors * [RUNTIME] changed kill process timeout from one second to three seconds * Start async hibike rewrite * [HIBIKE] Full implementation of async smart sensor protocol (#523) Now, SmartSensorProtocol automagically registers itself with Hibike when it connects. * Fix bugs related to virtual devices Essentially, we exclude existing serial ports from our scan for new ones, but this didn't extend to virtual devices, leading them to be added multiple times. The other bug was that "connection_lost()" could get called before a device was identified, triggering a key error when we tried to take it out of the devices map. This is now checked for. Add function to create heartbeat requests * Add async virtual devices In addition, async virtual devices send heartbeat requests too, although they don't do anything with the responses. * Don't block event loop * Don't block event loop on state queue * Port process tests to async * Use aiofiles for nonblocking IO * Allow profiling measurements * Use an external cobs library * Memoize a few hot functions * [HIBIKE] Pause reading of hibike messages when max size exceeded * [HIBIKE] implemented backpressure on device side * Update hibike_packet submodule url * Use http submodule url * Fix trivial test failures * Move hibike_packet so that import fails * Exclude new name of hibike_packet from linting * Unify runtime and hibike pipfiles * Bump required python version to 3.7 * Fix linter errors * Increase write attempts * Fix syntax error * Update comment * Bump runtime version * Switch to bash * Reduce flakiness of tests * Asyncio/writefix (#648) * [RUNTIME] new branch with new hibibke_write_value function * [RUNTIME] fixed hibike_write function * [RUNTIME] fixed hibike_write_value * changed an error name in runtime.py and cleand up runtime_Util.py a bit * changed terminate_process to use process.join() instead of busy-waiting * [HIBIKE] new pindefs for polarbear * [HIBIKE] Add warning to clearFault() * [HIBIKE] Most polarbear functionality done, still need to figure out what exact percentage the feedback pin gives * [HIBIKE] Modified readCurrent() in motor.cpp to output the correct amount of current * [HIBIKE] renaming new polarbear files appropriately * [HIBIKE] added polar bear to enumerations in hibikeDevices.json and hibike/devices.h * [HIBIKE] [RUNTIME] changed PolarBear dev ID, added PolarBear to statemanager device list * [RUNTIME] fixed a bug in studentapi where we returned a device's peripherals instead of it's name * [RUNTIME] uncommented process.terminate() in terminate_process() * [HIBIKE] Attempt at fixing one direction hiccups on motor controller * [HIBIKE] polarbear changed to use analog writes from the range of 0 to 1023 instead of pwm signals, also changed current limiting to 5 amps * [HIBIKE] Changed analog write range from 0 to 255 instead of 0 to 1023 * [HIBIKE] formatting cleaned, trying new method of updating pwm * MANUAL to MANUALDRIVE in current_limit * flashable new PolarBear package * [HIBIKE] Polarbears work now * Fixed lint issues --- .gitignore | 4 +- hibike/devices/PolarBear/LED.cpp | 78 ++++++ hibike/devices/PolarBear/LED.h | 12 + hibike/devices/PolarBear/PolarBear.cpp | 263 ++++++++++++++++++++ hibike/devices/PolarBear/PolarBear.h | 39 +++ hibike/devices/PolarBear/current_limit.cpp | 130 ++++++++++ hibike/devices/PolarBear/current_limit.h | 12 + hibike/devices/PolarBear/encoder.cpp | 55 ++++ hibike/devices/PolarBear/encoder.h | 13 + hibike/devices/PolarBear/motor.cpp | 84 +++++++ hibike/devices/PolarBear/motor.h | 19 ++ hibike/devices/PolarBear/pid.cpp | 134 ++++++++++ hibike/devices/PolarBear/pid.h | 26 ++ hibike/devices/PolarBear/pindefs.h | 25 ++ hibike/hibikeDevices.json | 17 ++ hibike/hibike_tests/hibike_process_tests.py | 16 +- hibike/lib/hibike/devices.h | 1 + hibike/lib/hibike/hibike_device.cpp | 238 +++++++++--------- runtime/runtime.py | 22 +- runtime/runtimeUtil.py | 3 +- runtime/statemanager.py | 1 + runtime/studentapi.py | 22 +- 22 files changed, 1070 insertions(+), 144 deletions(-) create mode 100644 hibike/devices/PolarBear/LED.cpp create mode 100644 hibike/devices/PolarBear/LED.h create mode 100644 hibike/devices/PolarBear/PolarBear.cpp create mode 100644 hibike/devices/PolarBear/PolarBear.h create mode 100644 hibike/devices/PolarBear/current_limit.cpp create mode 100644 hibike/devices/PolarBear/current_limit.h create mode 100644 hibike/devices/PolarBear/encoder.cpp create mode 100644 hibike/devices/PolarBear/encoder.h create mode 100644 hibike/devices/PolarBear/motor.cpp create mode 100644 hibike/devices/PolarBear/motor.h create mode 100644 hibike/devices/PolarBear/pid.cpp create mode 100644 hibike/devices/PolarBear/pid.h create mode 100644 hibike/devices/PolarBear/pindefs.h diff --git a/.gitignore b/.gitignore index 0a16b2b2..4a19cc18 100644 --- a/.gitignore +++ b/.gitignore @@ -397,5 +397,5 @@ hibike/virtual_devices.txt # Webstorm project settings .idea -# Visual Studio Code settings -.vscode/ +# Visual Studio Code config +.vscode diff --git a/hibike/devices/PolarBear/LED.cpp b/hibike/devices/PolarBear/LED.cpp new file mode 100644 index 00000000..cc938906 --- /dev/null +++ b/hibike/devices/PolarBear/LED.cpp @@ -0,0 +1,78 @@ +#include "LED.h" +#include "pindefs.h" +#include "encoder.h" +#include "current_limit.h" +#include "motor.h" +#include "Arduino.h" + +//place all LED control functions in here +void ctrl_LEDs() { + ctrl_RED(); + ctrl_YELLOW(); + ctrl_GREEN(); +} + +//decides when the red LED is on +void ctrl_RED() { + bool redOn = false; + + if(readVel() < 0 || !isMotorEnabled()) { + redOn = true; + } + + if(redOn) { + digitalWrite(LED_RED, HIGH); + } else { + digitalWrite(LED_RED, LOW); + } +} + +//decides when the yellow LED is on +void ctrl_YELLOW() { + bool yellowOn = false; + + if(read_limit_state() == 2 || read_limit_state() == 3 || !isMotorEnabled()) { //limit state number + yellowOn = true; + } + + if(yellowOn) { + digitalWrite(LED_YELLOW, HIGH); + } else { + digitalWrite(LED_YELLOW, LOW); + } +} + +//decides when the green LED is on +void ctrl_GREEN() { + bool greenOn = false; + + if(readVel() > 0 || !isMotorEnabled()) { + greenOn = true; + } + + if(greenOn) { + digitalWrite(LED_GREEN, HIGH); + } else { + digitalWrite(LED_GREEN, LOW); + } +} + +void test_LEDs() +{ + digitalWrite(LED_GREEN,HIGH); + digitalWrite(LED_RED,HIGH); + digitalWrite(LED_YELLOW,HIGH); + + delay(1000); + + digitalWrite(LED_GREEN,LOW); + digitalWrite(LED_RED,LOW); + digitalWrite(LED_YELLOW,LOW); +} + +void setup_LEDs() +{ + pinMode(LED_GREEN, OUTPUT); + pinMode(LED_RED, OUTPUT); + pinMode(LED_YELLOW, OUTPUT); +} diff --git a/hibike/devices/PolarBear/LED.h b/hibike/devices/PolarBear/LED.h new file mode 100644 index 00000000..399f189d --- /dev/null +++ b/hibike/devices/PolarBear/LED.h @@ -0,0 +1,12 @@ +#ifndef LED_H +#define LED_H + +//function prototypes +void ctrl_LEDs(); +void ctrl_RED(); +void ctrl_YELLOW(); +void ctrl_GREEN(); +void test_LEDs(); +void setup_LEDs(); + + #endif /* LED_H */ diff --git a/hibike/devices/PolarBear/PolarBear.cpp b/hibike/devices/PolarBear/PolarBear.cpp new file mode 100644 index 00000000..22183358 --- /dev/null +++ b/hibike/devices/PolarBear/PolarBear.cpp @@ -0,0 +1,263 @@ +#include "PolarBear.h" +#include "pid.h" +#include "encoder.h" +#include "current_limit.h" +#include "motor.h" +#include "LED.h" +#include + +//////////////// DEVICE UID /////////////////// +hibike_uid_t UID = { + POLAR_BEAR, // Device Type + 0x01, // Year + UID_RANDOM, // ID +}; +/////////////////////////////////////////////// + +//A space for constants. +float pwmInput = 0; //Value that is received from hibike and is the goal PWM +uint8_t driveMode = 0; + +void setup() { + motorSetup(); + //currentLimitSetup(); + encoderSetup(); + PIDSetup(); + setup_LEDs(); + test_LEDs(); + hibike_setup(); //use default heartbeat rates. look at /lib/hibike/hibike_device.cpp for exact values + motorDisable(); + driveMode = MANUALDRIVE; + // Enable the watchdog timer, with a one second period + wdt_reset(); + wdt_enable(WDTO_1S); +} + +void loop() { + ctrl_LEDs(); + hibike_loop(); + ctrl_pwm(); //actually drive the motor (function in current_limit.cpp) + wdt_reset(); +} + + + + +// You must implement this function. +// It is called when the device receives a Device Write packet. +// Updates param to new value passed in data. +// param - Parameter index +// data - value to write, in little-endian bytes +// len - number of bytes in data +// +// return - size of bytes written on success; otherwise return 0 + +uint32_t device_write(uint8_t param, uint8_t* data, size_t len) +{ + switch (param) { + + case DUTY_CYCLE: + motorEnable(); + disablePID(); + driveMode = MANUALDRIVE; + pwmInput = ((float *)data)[0]; + return sizeof(float); + break; + + case PID_POS_SETPOINT: + motorEnable(); + driveMode = PID_POS; + enablePos(); + setPosSetpoint(((float *)data)[0]); + return sizeof(float); + break; + + case PID_POS_KP: + setPosKP(((float *)data)[0]); + return sizeof(float); + break; + + case PID_POS_KI: + setPosKI(((float *)data)[0]); + return sizeof(float); + break; + + case PID_POS_KD: + setPosKD(((float *)data)[0]); + return sizeof(float); + break; + + case PID_VEL_SETPOINT: + motorEnable(); + driveMode = PID_VEL; + enableVel(); + setVelSetpoint(((float *)data)[0]); + return sizeof(float); + break; + + case PID_VEL_KP: + setVelKP(((float *)data)[0]); + return sizeof(float); + break; + + case PID_VEL_KI: + setVelKI(((float *)data)[0]); + return sizeof(float); + break; + + case PID_VEL_KD: + setVelKD(((float *)data)[0]); + return sizeof(float); + break; + + case CURRENT_THRESH: + setCurrentThreshold(((float *)data)[0]); + return sizeof(float); + break; + + case ENC_POS: + if ((float) data[0] == 0) { + resetEncoder(); + return sizeof(float); + } + break; + + case ENC_VEL: + break; + + case MOTOR_CURRENT: + break; + + case DEADBAND: + setDeadBand(((float *)data)[0]); + return sizeof(float); + break; + + default: + return 0; + } + return 0; +} + + +// You must implement this function. +// It is called when the device receives a Device Data Update packet. +// Modifies data_update_buf to contain the parameter value. +// param - Parameter index +// data_update_buf - buffer to return data in, little-endian +// buf_len - Maximum length of the buffer +// +// return - sizeof(value) on success; 0 otherwise + +uint8_t device_read(uint8_t param, uint8_t* data_update_buf, size_t buf_len) +{ + float* float_buf; + + switch (param) { + + case DUTY_CYCLE: + if(buf_len < sizeof(float)) { + return 0; + } + float_buf = (float *) data_update_buf; + float_buf[0] = readPWMInput(); + return sizeof(float); + break; + + case PID_POS_SETPOINT: + break; + + case PID_POS_KP: + break; + + case PID_POS_KI: + break; + + case PID_POS_KD: + break; + + case PID_VEL_SETPOINT: + break; + + case PID_VEL_KP: + break; + + case PID_VEL_KI: + break; + + case PID_VEL_KD: + break; + + case CURRENT_THRESH: + break; + + case ENC_POS: + if (buf_len < sizeof(float)) { + return 0; + } + float_buf = (float *) data_update_buf; + float_buf[0] = readPos(); + return sizeof(float); + break; + + case ENC_VEL: + if (buf_len < sizeof(float)) { + return 0; + } + float_buf = (float *) data_update_buf; + float_buf[0] = readVel(); + return sizeof(float); + break; + + case MOTOR_CURRENT: + if (buf_len < sizeof(float)) { + return 0; + } + float_buf = (float *) data_update_buf; + float_buf[0] = readCurrent(); + return sizeof(float); + break; + + case DEADBAND: + if (buf_len< sizeof(float)){ + return 0; + } + float_buf = (float *) data_update_buf; + float_buf[0] = readDeadBand(); + return sizeof(float); + break; + + default: + return 0; + break; + } + return 0; +} + + +float readPWMInput() +{ + return pwmInput; +} + +void resetPWMInput() +{ + pwmInput = 0; +} + +uint8_t readDriveMode() +{ + return driveMode; +} + +void resetDriveMode() +{ + driveMode = MANUALDRIVE; +} + +// You must implement this function. +// It is called when the BBB sends a message to the Smart Device tellinng the Smart Device to disable itself. +// Consult README.md, section 6, to see what exact functionality is expected out of disable. +void device_disable() { + motorDisable(); +} diff --git a/hibike/devices/PolarBear/PolarBear.h b/hibike/devices/PolarBear/PolarBear.h new file mode 100644 index 00000000..8eb0d449 --- /dev/null +++ b/hibike/devices/PolarBear/PolarBear.h @@ -0,0 +1,39 @@ +#ifndef POLARBEAR_H +#define POLARBEAR_H + +#include "hibike_device.h" //to get the hibike framework + +#define NUM_PARAMS 15 +typedef enum { + MANUALDRIVE = 0, + PID_VEL = 1, + PID_POS = 2 +} DriveModes; + +typedef enum { + DUTY_CYCLE = 0, + PID_POS_SETPOINT = 1, + PID_POS_KP = 2, + PID_POS_KI = 3, + PID_POS_KD = 4, + PID_VEL_SETPOINT = 5, + PID_VEL_KP = 6, + PID_VEL_KI = 7, + PID_VEL_KD = 8, + CURRENT_THRESH = 9, + ENC_POS = 10, + ENC_VEL = 11, + MOTOR_CURRENT = 12, + DEADBAND = 13 +} param; + + +// function prototypes +void setup(); +void loop(); +float readPWMInput(); +void resetPWMInput(); +void resetDriveMode(); +uint8_t readDriveMode(); + +#endif /* POLARBEAR_H */ diff --git a/hibike/devices/PolarBear/current_limit.cpp b/hibike/devices/PolarBear/current_limit.cpp new file mode 100644 index 00000000..159b5b7b --- /dev/null +++ b/hibike/devices/PolarBear/current_limit.cpp @@ -0,0 +1,130 @@ +#include //arduino libraries go in < +#include "current_limit.h" +#include "motor.h" //our own motor.h +#include "PolarBear.h" +#include "pid.h" + +//code that does current limiting. +//This will be called every time the motor controller decides to adjust the PWM to the motor. + +float current_threshold = 5.0; //threshold in amps +#define LIMITED 4 //when in the limit state PWM = PWM/LIMITED + +#define EXIT_MAX 250 //how many ms we want to be in CAUGHT_MAX before moving onto LIMIT state +#define EXIT_LIMIT 1000 //how many ms we want to be in LIMIT before moving onto SPIKE state +#define EXIT_SPIKE 500 //how many ms we want to be in SPIKE before moving onto either the STANDARD or LIMIT state + +int in_max = 0; //how many times we have been in the CAUGHT_MAX state +int above_threshold = 0; +int in_limit = 0; //how many times we have been in the LIMIT state +int in_spike = 0; //how many times we have been in the SPIKE state +int below_threshold = 0; + +int limit_state = 0; + +//FSM STATES +typedef enum { + STANDARD = 0, + CAUGHT_MAX = 1, + LIMIT = 2, + SPIKE = 3 +} limitState; + + +void setCurrentThreshold(float x) +{ + current_threshold = x; +} + +int read_limit_state() +{ + return limit_state; +} + +// takes the desired PWM input, then decides whether to limit the current outputs the PWM output actually written to the motor +int current_limiting (float pwm_in) { + + int pwm_sign = (pwm_in > 0) ? 1 : -1; //separate the sign to make calculations easier, we only care about magnitude + pwm_in *= (float) pwm_sign; + + float current_read = readCurrent(); + + switch (limit_state) { + + case STANDARD: //we allow the pwm to be passed through normally and check to see if the CURRENT ever spikes above the threshold + if (current_read > current_threshold) { + limit_state = CAUGHT_MAX; + } else { + if (above_threshold > 0) { + above_threshold--; + } + } + break; + + case CAUGHT_MAX: //we have seen the max and we check to see if we are above max for EXIT_MAX consecutive cycles and then we go to LIMIT to protect the motor + if (in_max > EXIT_MAX) { + if(above_threshold >= EXIT_MAX / 2) { + above_threshold = 0; + limit_state = LIMIT; + } else { + limit_state = STANDARD; + } + in_max = 0; + } + + if (current_read > current_threshold) { + above_threshold++; + } + in_max++; + break; + + case LIMIT: //we limit the pwm to 0.25 the value and wait EXIT_LIMIT cycles before attempting to spike and check the current again + pwm_in /= LIMITED; + if (in_limit > EXIT_LIMIT) { + in_limit = 0; + limit_state = SPIKE; + } else { + in_limit++; + } + break; + + case SPIKE: //we bring the pwm back to the target for a brief amount of time and check to see if it can continue in standard or if we need to continue limiting + if (in_spike > EXIT_SPIKE) { + if (below_threshold >= (EXIT_SPIKE/100)*99) { + limit_state = STANDARD; + } else { + limit_state = LIMIT; + } + below_threshold = 0; + in_spike = 0; + } else { + if (current_read < current_threshold) { + below_threshold++; + } + in_spike++; + } + break; + } + return pwm_in * pwm_sign; //returned signed and (possibly) limited pwm_input +} + +/* +void timerOneOps() { + current_limiting(); + drive(pwmOutput); +} + +void currentLimitSetup() { + Timer1.initialize(1000); + Timer1.attachInterrupt(timerOneOps); +} +*/ + +void ctrl_pwm() +{ + uint8_t drive_mode = readDriveMode(); + float pwm_in = (drive_mode == MANUALDRIVE) ? readPWMInput() : readPWMPID(); + //float pwm_out = current_limiting(pwm_in); + float pwm_out = pwm_in; + drive(pwm_out); +} diff --git a/hibike/devices/PolarBear/current_limit.h b/hibike/devices/PolarBear/current_limit.h new file mode 100644 index 00000000..4efb9c27 --- /dev/null +++ b/hibike/devices/PolarBear/current_limit.h @@ -0,0 +1,12 @@ +#ifndef CURRENT_LIMIT_H +#define CURRENT_LIMIT_H +#include + +//function prototypes +void currentLimitSetup(); +void setCurrentThreshold(float x); +int read_limit_state(); + +void ctrl_pwm(); + +#endif /* CURRENT_LIMIT_H */ diff --git a/hibike/devices/PolarBear/encoder.cpp b/hibike/devices/PolarBear/encoder.cpp new file mode 100644 index 00000000..8259fac5 --- /dev/null +++ b/hibike/devices/PolarBear/encoder.cpp @@ -0,0 +1,55 @@ +#include //arduino Quadrature Encoder library with 4x sensing +#include //Library for timer3 +#include "encoder.h" +#include "pindefs.h" + +//Code here that does position and velocity sensing +//This block of code is responsible for keeping track of the motor's position and velocity, without the say-so of any other blocks of code. + +//Velocity sensing will have its own independent timer soley for its own use, and also use interupts. + + +Encoder myEnc(encoder0PinA, encoder0PinB); + +// 2500/11 comes from 1000000 (1 sec) / 4400 (tick range) +long res = 100; +long interval_us = res*(2500/11); //interval in us +double pos = 0; +double vel = 0; +volatile signed long old_encoder0Pos = 0; + +void timerThreeOps() { + updatePos(); + updateVel(); +} + +void encoderSetup() { + + pinMode(encoder0PinA,INPUT); + pinMode(encoder0PinB,INPUT); + + Timer3.initialize(interval_us); + Timer3.attachInterrupt(timerThreeOps); +} + +void resetEncoder() { + myEnc.write(0); +} + +void updatePos() { + pos = myEnc.read(); +} + +double readPos() { + return pos; +} + +double readVel() { + return vel; +} + +void updateVel() { + double enc_reading = pos; + vel = ((enc_reading - old_encoder0Pos)*1000000)/((float) interval_us); + old_encoder0Pos = enc_reading; //calculate for the next vel calc +} diff --git a/hibike/devices/PolarBear/encoder.h b/hibike/devices/PolarBear/encoder.h new file mode 100644 index 00000000..2f8afed1 --- /dev/null +++ b/hibike/devices/PolarBear/encoder.h @@ -0,0 +1,13 @@ +#ifndef ENCODER_H +#define ENCODER_H + +//function prototypes +void encoderSetup(); +void resetEncoder(); +double readPos(); +double readVel(); +void updateVel(); +void updatePos(); + + +#endif /* ENCODER_H */ diff --git a/hibike/devices/PolarBear/motor.cpp b/hibike/devices/PolarBear/motor.cpp new file mode 100644 index 00000000..71d159bf --- /dev/null +++ b/hibike/devices/PolarBear/motor.cpp @@ -0,0 +1,84 @@ +#include "TimerOne.h" +#include "motor.h" +#include "pindefs.h" +#include "PolarBear.h" +#include "pid.h" +#include "encoder.h" + +//tab to handle all controls issued to the motor including driving and braking + +bool motorEnabled = false; +float deadBand = 0.05; + +void motorSetup() +{ + pinMode(feedback,INPUT); + pinMode(PWM1, OUTPUT); + pinMode(PWM2, OUTPUT); + pinMode(INV, OUTPUT); + digitalWrite(INV, LOW); + + motorEnable(); +} + +void motorEnable() +{ + clearFault(); + motorEnabled = true; +} + +void motorDisable() +{ + disablePID(); + resetPID(); + resetEncoder(); + resetPWMInput(); + resetDriveMode(); + motorEnabled = false; +} + +bool isMotorEnabled() +{ + return motorEnabled; +} + +//returns current in amps +float readCurrent() +{ + return (analogRead(feedback) / 0.0024); //Number was generated based on a few tests across multiple boards. Valid for majority of good boards +} + +//takes a value from -1 to 1 inclusive and writes to the motor and sets the PWM1 and PWM2 pins for direction +void drive(float target) +{ + if (target < -deadBand) { + digitalWrite(INV, LOW); + analogWrite(PWM1, 255); + analogWrite(PWM2, 255 + (int) (target * 255)); + } else if (target > deadBand) { + target = target * -1; + digitalWrite(INV, HIGH); + analogWrite(PWM1, 255); + analogWrite(PWM2, 255 + (int) (target * 255)); + } else { + analogWrite(PWM2, HIGH); + analogWrite(PWM1, HIGH); + } +} + + +void clearFault() +{ + analogWrite(PWM1, 255); + analogWrite(PWM2, 255); +} + +void setDeadBand(float range) +{ + deadBand = range; +} + +float readDeadBand() +{ + return deadBand; +} diff --git a/hibike/devices/PolarBear/motor.h b/hibike/devices/PolarBear/motor.h new file mode 100644 index 00000000..ed49e122 --- /dev/null +++ b/hibike/devices/PolarBear/motor.h @@ -0,0 +1,19 @@ +#ifndef MOTOR_H +#define MOTOR_H + +//function prototypes +void motorSetup(); +void motorEnable(); +void motorDisable(); +bool isMotorEnabled(); + +//returns current in amps +float readCurrent(); + +//takes a value from -1 to 1 inclusive and writes to the motor and sets the INA and INB pins for direction +void drive(float target); +void clearFault(); +void setDeadBand(float range); +float readDeadBand(); + +#endif /* MOTOR_H */ diff --git a/hibike/devices/PolarBear/pid.cpp b/hibike/devices/PolarBear/pid.cpp new file mode 100644 index 00000000..1dc8202e --- /dev/null +++ b/hibike/devices/PolarBear/pid.cpp @@ -0,0 +1,134 @@ +#include +#include +#include "pid.h" +#include "encoder.h" +#include "PolarBear.h" + +float PIDPos = 0; +float PIDPosKP = 1; +float PIDPosKI = 0; +float PIDPosKD = 0; + +float PIDVel = 0; +float PIDVelKP = 1; +float PIDVelKI = 0; +float PIDVelKD = 0; + +float pwmPID = 0; //Value that is generated by PID, if PID is disabled, this will be equal to pwmInput +double posForPID = 0; +double velForPID = 0; + +PID positionControl(&posForPID, (double*) &pwmPID, (double*) &PIDPos, (double) PIDPosKP, (double) PIDPosKI, (double) PIDPosKD, DIRECT); +PID velocityControl(&velForPID, (double*) &pwmPID, (double*) &PIDVel, (double) PIDVelKP, (double) PIDVelKI, (double) PIDVelKD, DIRECT); + +void PIDSetup() { + positionControl.SetOutputLimits(-1, 1); + positionControl.SetSampleTime(20); + velocityControl.SetOutputLimits(-1, 1); + velocityControl.SetSampleTime(20); + + FlexiTimer2::set(20, timerTwoOps); // call every 20ms. Beware, the API has other, more complicated, ways to set it up. + FlexiTimer2::start(); +} + +void timerTwoOps() { + uint8_t driveMode = readDriveMode(); + if (driveMode == PID_POS) { + posPID(); + } else if (driveMode == PID_VEL) { + velPID(); + } + +} + +float readPWMPID() { + return pwmPID; +} + +void resetPID() { + PIDPos = 0; + PIDPosKP = 1; + PIDPosKI = 0; + PIDPosKD = 0; + + PIDVel = 0; + PIDVelKP = 1; + PIDVelKI = 0; + PIDVelKD = 0; + + pwmPID = 0; //Value that is generated by PID, if PID is disabled, this will be equal to pwmInput + posForPID = 0; + velForPID = 0; +} + +void enablePos() { + positionControl.SetMode(AUTOMATIC); + velocityControl.SetMode(MANUAL); +} + +void enableVel() { + positionControl.SetMode(MANUAL); + velocityControl.SetMode(AUTOMATIC); +} + +void disablePID() { + positionControl.SetMode(MANUAL); + velocityControl.SetMode(MANUAL); +} + +void updatePosPID() { + positionControl.SetTunings(PIDPosKP, PIDPosKI, PIDPosKD); +} + +void updateVelPID() { + velocityControl.SetTunings(PIDVelKP, PIDVelKI, PIDVelKD); +} + +void posPID() { + posForPID = readPos(); + positionControl.Compute(); +} + +void velPID() { + velForPID = readVel(); + velocityControl.Compute(); +} + +void setPosSetpoint(float x) { + PIDPos = x; +} + +void setPosKP(float x) { + PIDPosKP = x; + updatePosPID(); +} + +void setPosKI(float x) { + PIDPosKI = x; + updatePosPID(); +} + +void setPosKD(float x) { + PIDPosKD = x; + updatePosPID(); +} + +void setVelSetpoint(float x) { + PIDVel = x; + +} + +void setVelKP(float x) { + PIDVelKP = x; + updateVelPID(); +} + +void setVelKI(float x) { + PIDVelKI = x; + updateVelPID(); +} + +void setVelKD(float x) { + PIDVelKD = x; + updateVelPID(); +} diff --git a/hibike/devices/PolarBear/pid.h b/hibike/devices/PolarBear/pid.h new file mode 100644 index 00000000..7d270f9c --- /dev/null +++ b/hibike/devices/PolarBear/pid.h @@ -0,0 +1,26 @@ +#ifndef PID_H +#define PID_H + + +// function prototypes +void PIDSetup(); +float readPWMPID(); +void timerTwoOps(); +void enablePos(); +void enableVel(); +void disablePID(); +void updatePosPID(); +void updateVelPID(); +void posPID(); +void velPID(); +void setPosSetpoint(float x); +void setPosKP(float x); +void setPosKI(float x); +void setPosKD(float x); +void setVelSetpoint(float x); +void setVelKP(float x); +void setVelKI(float x); +void setVelKD(float x); +void resetPID(); + +#endif /* PID_H */ diff --git a/hibike/devices/PolarBear/pindefs.h b/hibike/devices/PolarBear/pindefs.h new file mode 100644 index 00000000..f0e92f4b --- /dev/null +++ b/hibike/devices/PolarBear/pindefs.h @@ -0,0 +1,25 @@ +#ifndef PINDEFS_H +#define PINDEFS_H + + +//Pins. A0 is proper arduino syntax + +//leaving this here for reference +// #define PWM 9 +// #define enable_pin 21 +// #define enableAnalog A3 +// #define current_pin 8 +// #define INA 16 +// #define INB 10 + +#define PWM1 6 //now used to drive the thingy, used to be INA and INB +#define PWM2 9 +#define feedback A3 //indicator for safety, possible new current_pin? +#define LED_RED 2 //leds are just pin changes +#define LED_YELLOW 3 +#define LED_GREEN 4 +#define encoder0PinA 14 //encoders are the same fucntionality +#define encoder0PinB 15 +#define INV 5 //digitalwrite to low or else things will go backwards + +#endif /* PINDEFS_H */ diff --git a/hibike/hibikeDevices.json b/hibike/hibikeDevices.json index 1bc4d23e..1a3ef027 100644 --- a/hibike/hibikeDevices.json +++ b/hibike/hibikeDevices.json @@ -54,6 +54,23 @@ ] }, + {"id": 12, "name": "PolarBear", "params": [ + {"number": 0 , "name": "duty_cycle" , "type": "float" , "read": true , "write": true }, + {"number": 1 , "name": "pid_pos_setpoint" , "type": "float" , "read": false , "write": true }, + {"number": 2 , "name": "pid_pos_kp" , "type": "float" , "read": false , "write": true }, + {"number": 3 , "name": "pid_pos_ki" , "type": "float" , "read": false , "write": true }, + {"number": 4 , "name": "pid_pos_kd" , "type": "float" , "read": false , "write": true }, + {"number": 5 , "name": "pid_vel_setpoint" , "type": "float" , "read": false , "write": true }, + {"number": 6 , "name": "pid_vel_kp" , "type": "float" , "read": false , "write": true }, + {"number": 7 , "name": "pid_vel_ki" , "type": "float" , "read": false , "write": true }, + {"number": 8 , "name": "pid_vel_kd" , "type": "float" , "read": false , "write": true }, + {"number": 9 , "name": "current_thresh" , "type": "float" , "read": false , "write": true }, + {"number": 10 , "name": "enc_pos" , "type": "float" , "read": true , "write": true }, + {"number": 11 , "name": "enc_vel" , "type": "float" , "read": true , "write": false }, + {"number": 12 , "name": "motor_current" , "type": "float" , "read": true , "write": false }, + {"number": 13 , "name": "deadband" , "type": "float" , "read": true , "write": true } + ] + }, {"id": 10, "name": "YogiBear", "params": [ {"number": 0 , "name": "duty_cycle" , "type": "float" , "read": true , "write": true }, diff --git a/hibike/hibike_tests/hibike_process_tests.py b/hibike/hibike_tests/hibike_process_tests.py index 6b419958..72d8b6ed 100644 --- a/hibike/hibike_tests/hibike_process_tests.py +++ b/hibike/hibike_tests/hibike_process_tests.py @@ -17,7 +17,6 @@ import hibike_message as hm from hibike_tester import Hibike - def add_runtime_to_path(): """ Enable import of runtime modules. @@ -163,7 +162,7 @@ def read(self, uid: int, param: str): """ for _ in range(self.READ_WRITE_ATTEMPTS): self.hibike.read(uid, [param]) - time.sleep(self.READ_WRITE_DELAY) + time.sleep(self.READ_WRITE_DELAY) return self.hibike.get_last_cached(uid, param) def test_write_then_read(self): @@ -178,6 +177,19 @@ def test_write_then_read(self): hibike_value = self.read(uid, "duty_cycle") self.assertAlmostEqual(hibike_value, duty_cycle_val) + def test_subscribe_write(self): + """ + Test that subscribing sends up to date values. + """ + self.hibike.subscribe_all() + for (uid, dev_type) in self.hibike.get_uids_and_types(): + if dev_type == "YogiBear": + self.write(uid, [("duty_cycle", random.random())]) + new_val = random.random() + self.write(uid, [("duty_cycle", new_val)]) + new_hibike_val = self.hibike.get_last_cached(uid, "duty_cycle") + self.assertAlmostEqual(new_hibike_val, new_val) + def test_nonexistent_device(self): """ Nonexistent device operations should error. diff --git a/hibike/lib/hibike/devices.h b/hibike/lib/hibike/devices.h index 962b97cd..1863d490 100644 --- a/hibike/lib/hibike/devices.h +++ b/hibike/lib/hibike/devices.h @@ -8,6 +8,7 @@ typedef enum { BATTERY_BUZZER = 0x04, TEAM_FLAG = 0x05, YOGI_BEAR = 0x0A, + POLAR_BEAR = 0x0C, RFID = 0x0B, SERVO_CONTROL = 0x07, COLOR_SENSOR = 0x09, diff --git a/hibike/lib/hibike/hibike_device.cpp b/hibike/lib/hibike/hibike_device.cpp index c8d18b03..0ca10f88 100644 --- a/hibike/lib/hibike/hibike_device.cpp +++ b/hibike/lib/hibike/hibike_device.cpp @@ -25,27 +25,27 @@ const float MIN_SUB_DELAY_MS = 40.0f; const float ALPHA = 0.25f; void update_sub_delay(void) { - // Clamp queue_fullness just in case it's greater than 100 - queue_fullness = min(queue_fullness, 100); - // Interpolate between the maximum and minimum delay - float new_delay = max(MAX_SUB_DELAY_MS * queue_fullness / 100, MIN_SUB_DELAY_MS); - sub_delay = (uint16_t)(ALPHA * sub_delay + (1 - ALPHA) * new_delay); + // Clamp queue_fullness just in case it's greater than 100 + queue_fullness = min(queue_fullness, 100); + // Interpolate between the maximum and minimum delay + float new_delay = max(MAX_SUB_DELAY_MS * queue_fullness / 100, MIN_SUB_DELAY_MS); + sub_delay = (uint16_t)(ALPHA * sub_delay + (1 - ALPHA) * new_delay); } void hibike_setup(uint32_t _disable_latency, uint32_t _heartbeat_delay) { - //heartbeat_delay to 0 to not send heartbeat Requests - //disable_latency to 0 to not disable on lack of heartbeats. - Serial.begin(115200); - prev_sub_time = millis(); - sub_delay = 0; //0 is a default that means that nothing is subscribed to it. - disable_latency = _disable_latency; //this long without recieving a heartbeat response to disable myself. - heartbeat_delay = _heartbeat_delay; //Send a quest for heartbeat this fast. - - // Setup Error LED - pinMode(LED_PIN, OUTPUT); - digitalWrite(LED_PIN, LOW); - heartbeat_state = RESTING; - led_enabled = false; + //heartbeat_delay to 0 to not send heartbeat Requests + //disable_latency to 0 to not disable on lack of heartbeats. + Serial.begin(115200); + prev_sub_time = millis(); + sub_delay = 0; //0 is a default that means that nothing is subscribed to it. + disable_latency = _disable_latency; //this long without recieving a heartbeat response to disable myself. + heartbeat_delay = _heartbeat_delay; //Send a quest for heartbeat this fast. + + // Setup Error LED + pinMode(LED_PIN, OUTPUT); + digitalWrite(LED_PIN, LOW); + heartbeat_state = RESTING; + led_enabled = false; } void hibike_setup() //call hibike_setup with default values. Useful since most devices will probably do it this way @@ -54,109 +54,107 @@ void hibike_setup() //call hibike_setup with default values. Useful since most } void hibike_loop() { - curr_time = millis(); - // Check for Hibike packets - if (Serial.available() > 1) { - if (read_message(&hibike_buff) == -1) { - toggleLED(); - } else { - int offset; - switch (hibike_buff.messageID) { - - case SUBSCRIPTION_REQUEST: - // change sub_delay and send SUB_RESP - sub_delay = *((uint16_t*) &hibike_buff.payload[2]); - params = *((uint16_t*) &hibike_buff.payload[0]); - send_subscription_response(params, sub_delay, &UID); - break; - - case SUBSCRIPTION_RESPONSE: - // Unsupported packet - toggleLED(); - break; - - case DEVICE_WRITE: - //loop over params - old_params = params; - offset = 2; - params = *((uint16_t*)&hibike_buff.payload[0]); - for (uint16_t count = 0; (params >> count) > 0; count++) { - if (params & (1< 0) && (curr_time - prev_sub_time >= sub_delay)) { - prev_sub_time = curr_time; - send_data_update(params); - } - - if ( ( (heartbeat_delay) > 0) && ( (curr_time - prevHeartTimer) >= heartbeat_delay) ) - { - //send heartbeat request - send_heartbeat_request(1); //1 is just a placeholder for future use. - prevHeartTimer = curr_time; - } - - if ( (disable_latency > 0) && ( (curr_time - resp_heartbeat) >= disable_latency) ) - { - device_disable(); //on sensor specific device - } + curr_time = millis(); + // Check for Hibike packets + if (Serial.available() > 1) { + if (read_message(&hibike_buff) == -1) { + toggleLED(); + } else { + int offset; + switch (hibike_buff.messageID) { + + case SUBSCRIPTION_REQUEST: + // change sub_delay and send SUB_RESP + sub_delay = *((uint16_t*) &hibike_buff.payload[2]); + params = *((uint16_t*) &hibike_buff.payload[0]); + send_subscription_response(params, sub_delay, &UID); + break; + + case SUBSCRIPTION_RESPONSE: + // Unsupported packet + toggleLED(); + break; + + case DEVICE_WRITE: + //loop over params + old_params = params; + offset = 2; + params = *((uint16_t*)&hibike_buff.payload[0]); + for (uint16_t count = 0; (params >> count) > 0; count++) { + if (params & (1< 0) && (curr_time - prev_sub_time >= sub_delay)) { + prev_sub_time = curr_time; + send_data_update(params); + } + + if (((heartbeat_delay) > 0) && ((curr_time - prevHeartTimer) >= heartbeat_delay)) { + //send heartbeat request + send_heartbeat_request(1); //1 is just a placeholder for future use. + prevHeartTimer = curr_time; + } + + if ((disable_latency > 0) && ((curr_time - resp_heartbeat) >= disable_latency)) { + device_disable(); //on sensor specific device + } } void toggleLED() { - if (led_enabled) { - digitalWrite(LED_PIN, LOW); - led_enabled = false; - } else { - digitalWrite(LED_PIN, HIGH); - led_enabled = true; - } + if (led_enabled) { + digitalWrite(LED_PIN, LOW); + led_enabled = false; + } else { + digitalWrite(LED_PIN, HIGH); + led_enabled = true; + } } diff --git a/runtime/runtime.py b/runtime/runtime.py index 8486292e..30575bfc 100644 --- a/runtime/runtime.py +++ b/runtime/runtime.py @@ -7,7 +7,6 @@ import re import signal import sys -import time import traceback import warnings @@ -374,17 +373,18 @@ def terminate_process(process_name): if process_name not in ALL_PROCESSES: return process = ALL_PROCESSES.pop(process_name) + # TODO: make sure that processes aren't sharing queues and pipes process.terminate() - for _ in range(300): # Gives 3 seconds for process to terminate - time.sleep(.01) # Give the OS a chance to terminate the other process - if not process.is_alive(): - break - if process.is_alive(): - print("Terminating with EXTREME PREJUDICE") - print("Queue state is probably boned and we should restart entire runtime") - print("Boned Process:", process_name) - os.kill(process.pid, signal.SIGKILL) - raise NotImplementedError + process.join(3) # Give process 3 seconds to terminate + if process.exitcode != None: # process has terminated + return + else: + if process.is_alive(): + print("Terminating with EXTREME PREJUDICE") + print("Queue state is probably boned and we should restart entire runtime") + print("Boned Process:", process_name) + os.kill(process.pid, signal.SIGKILL) + raise OSError def runtime_test(test_names): diff --git a/runtime/runtimeUtil.py b/runtime/runtimeUtil.py index 6c523c06..bdacfd73 100644 --- a/runtime/runtimeUtil.py +++ b/runtime/runtimeUtil.py @@ -26,7 +26,7 @@ class RUNTIME_CONFIG(Enum): """Assorted runtime constants.""" STUDENT_CODE_TIMELIMIT = 1 STUDENT_CODE_HZ = 20 # Number of times to execute studentCode.main per second - DEBUG_DELIMITER_STRING = "****************** RUNTIME DEBUG ******************" + DEBUG_DELIMITER_STRING = "\n****************** RUNTIME MESSAGE ******************" PIPE_READY = ["ready"] TEST_OUTPUT_DIR = "test_outputs/" VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH = __version__ @@ -47,6 +47,7 @@ class BAD_EVENTS(Enum): UDP_SEND_ERROR = "UDPSend Process Crashed" UDP_RECV_ERROR = "UDPRecv Process Crashed" TCP_ERROR = "TCP Process Crashed" + HIBIKE_START_ERROR = "Hibike Process failed to start" ENTER_TELEOP = "Dawn says enter Teleop" ENTER_AUTO = "Dawn says enter Auto" ENTER_IDLE = "Dawn says enter Idle" diff --git a/runtime/statemanager.py b/runtime/statemanager.py index e8d09276..fc8aa794 100644 --- a/runtime/statemanager.py +++ b/runtime/statemanager.py @@ -33,6 +33,7 @@ def make_subscription_map(): "BatteryBuzzer": ["v_batt", "is_unsafe"], "ServoControl": ["servo0", "servo1"], "YogiBear": ["duty_cycle", "enc_pos", "enc_vel"], + "PolarBear": ["duty_cycle", "enc_pos", "enc_vel"], "RFID": ["id", "tag_detect"], "ExampleDevice": ["hazuki", "sapphire", "reina", "asuka"] } diff --git a/runtime/studentapi.py b/runtime/studentapi.py index e62f590b..99606f27 100644 --- a/runtime/studentapi.py +++ b/runtime/studentapi.py @@ -92,6 +92,8 @@ class Robot(StudentAPI): "ServoControl": ["servo0", "servo1"], "YogiBear": ["duty_cycle", "pid_pos_setpoint", "pid_pos_kp", "pid_pos_ki", "pid_pos_kd", "current_thresh", "enc_pos"], + "PolarBear": ["duty_cycle", "pid_pos_setpoint", "pid_pos_kp", "pid_pos_ki", + "pid_pos_kd", "current_thresh", "enc_pos"], } deviceName_to_readParams = { "LimitSwitch": ["switch0", "switch1", "switch2"], @@ -99,6 +101,7 @@ class Robot(StudentAPI): "Potentiometer": ["pot0", "pot1", "pot2"], "ServoControl": ["servo0", "servo1"], "YogiBear": ["duty_cycle", "enc_pos", "enc_vel"], + "PolarBear": ["duty_cycle", "enc_pos", "enc_vel"], "RFID": ["id", "tag_detect"], } param_to_valid_values = { @@ -125,6 +128,9 @@ def __init__(self, to_manager, from_manager, func_map): self._stdout_buffer = io.StringIO() self._get_all_sensors() + + self.student_code_writes = {} + def _get_all_sensors(self): """Get a list of sensors.""" self.peripherals = self._get_sm_value('hibike', 'devices') @@ -140,20 +146,19 @@ def set_value(self, device_name, param, value): uid = self._hibike_get_uid(device_name) self._check_write_params(uid, param) self._check_value(param, value) - self.to_manager.put([HIBIKE_COMMANDS.WRITE, [uid, [(param, value)]]]) + self.hibike_write_value(uid, [(param, value)]) def set_motor(self, device_name, value): uid = self._hibike_get_uid(device_name) self._check_write_params(uid, "duty_cycle") self._check_value("duty_cycle", value) - self.to_manager.put([HIBIKE_COMMANDS.WRITE, [uid, [("duty_cycle", value)]]]) + self.hibike_write_value(uid, [("duty_cycle", value)]) def stop_motor(self, device_name): uid = self._hibike_get_uid(device_name) self._check_write_params(uid, "duty_cycle") self._check_value("duty_cycle", 0) - self.to_manager.put([HIBIKE_COMMANDS.WRITE, [uid, [("duty_cycle", 0)]]]) - + self.hibike_write_value(uid, [("duty_cycle", 0)]) def run(self, func, *args, **kwargs): """ @@ -254,9 +259,7 @@ def _hibike_get_uid(self, name): try: # TODO: Implement sensor mappings, right now uid is the number (or string of number) device = int(name) - if device in self.peripherals: - return device - raise KeyError(str(device)) + return device except (ValueError, KeyError) as exc: raise StudentAPIKeyError('Device not found: ' + str(name)) from exc @@ -282,7 +285,10 @@ def _send_prints(self): def hibike_write_value(self, uid, params): """Writes parameters to ``uid``.""" - self.to_manager.put([HIBIKE_COMMANDS.WRITE, [uid, params]]) + #create global variable dictionary student_code_writes + if self.student_code_writes.get(uid) != params: + self.student_code_writes[uid] = params + self.to_manager.put([HIBIKE_COMMANDS.WRITE, [uid, params]]) def _get_gamecodes(self): """Get a gamecode."""