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
pioneers · Feb 28, 2019 · 02d4d00 · 02d4d00
1 parent 8e22e11
commit 02d4d00
Show file tree

Hide file tree

Showing 22 changed files with 1,070 additions and 144 deletions.
diff --git 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
@@ -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
@@ -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
@@ -0,0 +1,263 @@
+#include "PolarBear.h"
+#include "pid.h"
+#include "encoder.h"
+#include "current_limit.h"
+#include "motor.h"
+#include "LED.h"
+#include <avr/wdt.h>
+
+//////////////// 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();
+}