Last active
August 7, 2020 19:12
-
-
Save kubark42/4e895f9f58af44e06acd594b8d393e3b to your computer and use it in GitHub Desktop.
Thrust_stand_control_1780.ipynb
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
{ | |
"cells": [ | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Boilerplate python\n", | |
"from __future__ import absolute_import, division, print_function\n", | |
"from builtins import * # @UnusedWildImport\n", | |
"from pprint import pprint\n", | |
"\n", | |
"import math\n", | |
"import numbers\n", | |
"\n", | |
"import binascii\n", | |
"from cffi import FFI\n", | |
"import threading\n", | |
"import queue\n", | |
"import pause\n", | |
"\n", | |
"import time\n", | |
"import datetime\n", | |
"\n", | |
"import random\n" | |
], | |
"outputs": [], | |
"execution_count": 69, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:32:01.710Z", | |
"iopub.execute_input": "2020-08-06T23:32:01.721Z", | |
"iopub.status.idle": "2020-08-06T23:32:01.736Z", | |
"shell.execute_reply": "2020-08-06T23:32:01.747Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# ESC & Motor" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"numMagneticPoles = 2\n", | |
"motorAVoltageScaling = 1\n", | |
"motorACurrentScaling = 1\n", | |
"motorAThrustScaling = 1" | |
], | |
"outputs": [], | |
"execution_count": 78, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:37:11.132Z", | |
"iopub.execute_input": "2020-08-06T23:37:11.144Z", | |
"iopub.status.idle": "2020-08-06T23:37:11.163Z", | |
"shell.execute_reply": "2020-08-06T23:37:11.179Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Measurement Computing\n", | |
"\n", | |
"First configure the Measurement Computing DAQ module" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"from mcculw import ul\n", | |
"from mcculw.enums import TempScale\n", | |
"from mcculw.ul import ULError\n", | |
"from mcculw.enums import InterfaceType" | |
], | |
"outputs": [], | |
"execution_count": 41, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:22.741Z", | |
"iopub.execute_input": "2020-08-06T23:26:22.749Z", | |
"shell.execute_reply": "2020-08-06T23:26:22.767Z", | |
"iopub.status.idle": "2020-08-06T23:26:22.760Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Define channels we wish to monitor\n", | |
"temperatureChannels = [0,1,2,3,4,5]" | |
], | |
"outputs": [], | |
"execution_count": 42, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:22.953Z", | |
"iopub.execute_input": "2020-08-06T23:26:22.960Z", | |
"iopub.status.idle": "2020-08-06T23:26:22.972Z", | |
"shell.execute_reply": "2020-08-06T23:26:22.980Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"def config_first_detected_device(board_num):\n", | |
" \"\"\"Adds the first available device to the UL.\n", | |
"\n", | |
" Parameters\n", | |
" ----------\n", | |
" board_num : int, optional\n", | |
" The board number to assign to the board when configuring the device.\n", | |
"\n", | |
" Returns\n", | |
" -------\n", | |
" boolean\n", | |
" True if a device was found and added, False if no devices were\n", | |
" found. \n", | |
" \"\"\"\n", | |
"\n", | |
" # Get the device inventory\n", | |
" devices = ul.get_daq_device_inventory(InterfaceType.ANY)\n", | |
" # Check if any devices were found\n", | |
" if len(devices) > 0:\n", | |
" device = devices[0]\n", | |
" # Print a messsage describing the device found\n", | |
" print(\"Found device: \" + device.product_name +\n", | |
" \" (\" + device.unique_id + \")\\n\")\n", | |
" # Add the device to the UL.\n", | |
" ul.create_daq_device(board_num, device)\n", | |
" return True\n", | |
"\n", | |
" return False\n", | |
" \n", | |
"def readTemperature(channel):\n", | |
" value = [None] * len(channel)\n", | |
" \n", | |
" for i in channel:\n", | |
" try:\n", | |
" value[i] = ul.t_in(board_num, i, TempScale.CELSIUS)\n", | |
" except:\n", | |
" value[i] = 'Disconnected'\n", | |
" return value\n" | |
], | |
"outputs": [], | |
"execution_count": 43, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:23.167Z", | |
"iopub.execute_input": "2020-08-06T23:26:23.175Z", | |
"iopub.status.idle": "2020-08-06T23:26:23.188Z", | |
"shell.execute_reply": "2020-08-06T23:26:23.196Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# It's unlikely we acquire a second Measurement Computing board, so for the moment it's safe to assume it's the only board present.\n", | |
"board_num = 0\n", | |
"\n", | |
"# Try to automatically find and configure device. No fall back in case this fails!\n", | |
"ul.ignore_instacal()\n", | |
"if not config_first_detected_device(board_num):\n", | |
" print(\"Could not find device.\")\n" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"Found device: USB-TEMP (1A6A4A4)\n", | |
"\n" | |
] | |
} | |
], | |
"execution_count": 44, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:23.380Z", | |
"iopub.execute_input": "2020-08-06T23:26:23.388Z", | |
"iopub.status.idle": "2020-08-06T23:26:23.507Z", | |
"shell.execute_reply": "2020-08-06T23:26:23.556Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Perform a quick sample of all channels. This will print an error if any channels are unconnected to thermocouples\n", | |
"temperatureReading = readTemperature(temperatureChannels)\n", | |
"\n", | |
"print(temperatureReading)" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"[26.51288414001465, 'Disconnected', 'Disconnected', 'Disconnected', 'Disconnected', 'Disconnected']\n" | |
] | |
} | |
], | |
"execution_count": 45, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:23.570Z", | |
"iopub.execute_input": "2020-08-06T23:26:23.580Z", | |
"iopub.status.idle": "2020-08-06T23:26:23.596Z", | |
"shell.execute_reply": "2020-08-06T23:26:23.637Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# LabJack" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"import u3" | |
], | |
"outputs": [], | |
"execution_count": 46, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:23.973Z", | |
"iopub.execute_input": "2020-08-06T23:26:23.981Z", | |
"shell.execute_reply": "2020-08-06T23:26:24.001Z", | |
"iopub.status.idle": "2020-08-06T23:26:23.994Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"TimerClockBase = 6 # 6 is the enum for 48MHz\n", | |
"\n", | |
"TimerFrequency = 48e6 # 48 Mhz\n", | |
"\n", | |
"# PWM frequency = (TimerFrequency / 2^16) / TimerClockDivisor\n", | |
"TimerClockDivisor = 2\n", | |
"\n", | |
"SINGLE_ENDED = 31\n", | |
"SPECIAL_3V6_RANGE = 32" | |
], | |
"outputs": [], | |
"execution_count": 47, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:24.212Z", | |
"iopub.execute_input": "2020-08-06T23:26:24.219Z", | |
"iopub.status.idle": "2020-08-06T23:26:24.230Z", | |
"shell.execute_reply": "2020-08-06T23:26:24.237Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"try:\n", | |
" d = u3.U3()\n", | |
" pprint(d.getCalibrationData())\n", | |
"except Exception as e:\n", | |
" print(\"[ERROR]: \"+ str(e))" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"{'dac0Offset': -0.6108887793961912,\n", | |
" 'dac0Slope': 51.65718947863206,\n", | |
" 'dac1Offset': -0.8383769274223596,\n", | |
" 'dac1Slope': 51.63090718537569,\n", | |
" 'hvAIN0Offset': -0.42649758444167674,\n", | |
" 'hvAIN0Slope': 7.048225961625576e-05,\n", | |
" 'hvAIN1Offset': -0.4109869464300573,\n", | |
" 'hvAIN1Slope': 7.006712257862091e-05,\n", | |
" 'hvAIN2Offset': -0.42627710197120905,\n", | |
" 'hvAIN2Slope': 7.04785343259573e-05,\n", | |
" 'hvAIN3Offset': -0.41075132600963116,\n", | |
" 'hvAIN3Slope': 7.006386294960976e-05,\n", | |
" 'lvDiffOffset': -2.4595058413688093,\n", | |
" 'lvDiffSlope': 7.478427141904831e-05,\n", | |
" 'lvSEOffset': 0.0038205122109502554,\n", | |
" 'lvSESlope': 3.7300167605280876e-05,\n", | |
" 'tempSlope': 0.012705469038337469,\n", | |
" 'vRef1.5AtCal': 0.0,\n", | |
" 'vRefAtCAl': 2.45870645577088,\n", | |
" 'vRegAtCal': 0.0}\n" | |
] | |
} | |
], | |
"execution_count": 48, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:24.466Z", | |
"iopub.execute_input": "2020-08-06T23:26:24.476Z", | |
"iopub.status.idle": "2020-08-06T23:26:24.496Z", | |
"shell.execute_reply": "2020-08-06T23:26:24.507Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Configure LabJack IO ports\n", | |
"# This register sets individual ports (from 1..8) to analog (1) or digital (0). \n", | |
"# Therefor, 0b00010011 has the 1st, 2nd, and 5th ports as analog inputs and the others as digital inputs\n", | |
"FIOAnalog = 0b00001111\n", | |
"\n", | |
"# Execute configuration\n", | |
"d.configIO(FIOAnalog=FIOAnalog, NumberOfTimersEnabled=1, TimerCounterPinOffset = 4, EnableCounter1=True)\n", | |
"\n", | |
"# Configure Timers\n", | |
"d.configTimerClock(TimerClockBase=TimerClockBase, TimerClockDivisor=TimerClockDivisor)" | |
], | |
"outputs": [ | |
{ | |
"output_type": "execute_result", | |
"execution_count": 49, | |
"data": { | |
"text/plain": [ | |
"{'TimerClockBase': 6, 'TimerClockDivisor': 2}" | |
] | |
}, | |
"metadata": {} | |
} | |
], | |
"execution_count": 49, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:24.640Z", | |
"iopub.execute_input": "2020-08-06T23:26:24.649Z", | |
"iopub.status.idle": "2020-08-06T23:26:24.666Z", | |
"shell.execute_reply": "2020-08-06T23:26:24.674Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"\n", | |
"\n", | |
"bits = d.getFeedback(u3.AIN(0, 31))[0]\n", | |
"d.binaryToCalibratedAnalogVoltage(bits, isLowVoltage=True, isSingleEnded = True, isSpecialSetting=True)\n", | |
"\n", | |
"print(d.binaryToCalibratedAnalogVoltage(bits, isSpecialSetting=True))\n", | |
"print(bits)\n", | |
"\n", | |
"d.setFIOState(7, state = 1)\n", | |
"d.getAIN(0,32)\n" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"0.930115225026384\n", | |
"12448\n" | |
] | |
}, | |
{ | |
"output_type": "execute_result", | |
"execution_count": 50, | |
"data": { | |
"text/plain": [ | |
"1.9974363467190415" | |
] | |
}, | |
"metadata": {} | |
} | |
], | |
"execution_count": 50, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:24.793Z", | |
"iopub.execute_input": "2020-08-06T23:26:24.802Z", | |
"iopub.status.idle": "2020-08-06T23:26:24.827Z", | |
"shell.execute_reply": "2020-08-06T23:26:24.836Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# RCBenchmark definitions" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"ffi = FFI()\n", | |
"\n", | |
"# Define a set of structures taken from https://gitlab.com/TytoRobotics/TestPlatformFirmware/-/blob/v2.10/TestPlatformFirmware/types.h\n", | |
"\n", | |
"ffi.cdef(\"\"\"\n", | |
"#define TEMP_PROBES_QTY 6 //how many temp probes we support (dynamic)\n", | |
"\n", | |
"//Board info\n", | |
"typedef struct {\n", | |
" uint8_t version;\n", | |
" uint8_t id[8];\n", | |
" uint8_t pSensorAvailable;\n", | |
" uint8_t tempProbeQty;\n", | |
" uint8_t tempProbes[TEMP_PROBES_QTY][8];\n", | |
" uint8_t s1780_PSA : 1;\n", | |
" uint8_t s1780_LCA : 1;\n", | |
" uint8_t s1780_PSB : 1;\n", | |
" uint8_t s1780_LCB : 1;\n", | |
" uint8_t s1780_PSA_BoardVersion;\n", | |
" uint8_t s1780_LCA_BoardVersion;\n", | |
" uint8_t s1780_PSB_BoardVersion;\n", | |
" uint8_t s1780_LCB_BoardVersion;\n", | |
"} board_info_t;\n", | |
"\n", | |
"//Information about this firwmare\n", | |
"typedef struct {\n", | |
"\tuint8_t com_protocol_version, major, minor;\n", | |
"} firmware_info_t;\n", | |
"\n", | |
"//Control inputs from PC for the s1780\n", | |
"typedef struct {\n", | |
" uint16_t A_PWM;\n", | |
" uint16_t A_Servo;\n", | |
" uint16_t B_PWM;\n", | |
" uint16_t B_Servo;\n", | |
" uint8_t reverseCutoffSwitch;\n", | |
"} s1780_control_t;\n", | |
"\n", | |
"//s1780 polled sensors\n", | |
"typedef struct {\n", | |
" float A_voltage; //in V\n", | |
" float A_current; //in A\n", | |
" float A_forces_N[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
" //float A_brushless_hz; //motor brushless_hz\n", | |
" float A_optical_hz; //motor optical_hz\n", | |
" float tempC[TEMP_PROBES_QTY]; //temperature probes\n", | |
" int16_t pressureP; //converted to the correct unit by the GUI\n", | |
" int16_t pressureT; //converted to the correct unit by the GUI\n", | |
" float B_voltage; //in V\n", | |
" float B_current; //in A\n", | |
" float B_forces_N[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
" //float A_brushless_hz; //motor brushless_hz\n", | |
" float B_optical_hz; //motor optical_hz\n", | |
" uint8_t overload : 1;\n", | |
" uint8_t limitswitch : 1;\n", | |
"} s1780_sensors_t;\n", | |
"\n", | |
"//s1780 extra debug\n", | |
"typedef struct {\n", | |
" float A_raw[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
" float B_raw[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
"} s1780_debug_t;\n", | |
"\n", | |
"\"\"\", packed=True) # `packed=False` ensures that the values are not byte-aligned. Typically in another bytestream these structs would be packed, but perhaps the developers at RCBenchmark did not think about doing that\n", | |
"\n", | |
"sensorData = ffi.new(\"s1780_sensors_t*\")\n", | |
"controlData = ffi.new(\"s1780_control_t*\")\n", | |
"firmwareData = ffi.new(\"firmware_info_t*\")\n", | |
"boardData = ffi.new(\"board_info_t*\")\n", | |
"debugData = ffi.new(\"s1780_debug_t*\")" | |
], | |
"outputs": [], | |
"execution_count": 51, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:25.146Z", | |
"iopub.execute_input": "2020-08-06T23:26:25.155Z", | |
"iopub.status.idle": "2020-08-06T23:26:25.170Z", | |
"shell.execute_reply": "2020-08-06T23:26:25.193Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Serial port\n", | |
"\n", | |
"Configure the serial port to dialog with the RCBenchmark software" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"import serial" | |
], | |
"outputs": [], | |
"execution_count": 52, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:25.757Z", | |
"iopub.execute_input": "2020-08-06T23:26:25.765Z", | |
"iopub.status.idle": "2020-08-06T23:26:25.778Z", | |
"shell.execute_reply": "2020-08-06T23:26:25.787Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"port = \"COM10\"\n", | |
"\n", | |
"# Open serial port\n", | |
"try:\n", | |
" serialPort = serial.Serial(port = port, baudrate=250000, timeout=0)\n", | |
"except Exception as e:\n", | |
" print(\"[ERROR]: \" + str(e))" | |
], | |
"outputs": [], | |
"execution_count": 53, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:26.479Z", | |
"iopub.execute_input": "2020-08-06T23:26:26.486Z", | |
"iopub.status.idle": "2020-08-06T23:26:26.496Z", | |
"shell.execute_reply": "2020-08-06T23:26:26.504Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# RCBenchmark Serial Protocol" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"def processRCBenchmarkCommand(command, payload):\n", | |
" # Ignore [Command = 0] as there's nothing to process\n", | |
" if command == 0: \n", | |
" return\n", | |
" \n", | |
" # [Command = 5] contains throttle commands\n", | |
" if command == 5:\n", | |
" # Copy the payload to the control data structure\n", | |
" D_buffer = ffi.buffer(controlData)\n", | |
" D_buffer[:] = payload\n", | |
" \n", | |
" return" | |
], | |
"outputs": [], | |
"execution_count": 54, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:27.318Z", | |
"iopub.execute_input": "2020-08-06T23:26:27.326Z", | |
"iopub.status.idle": "2020-08-06T23:26:27.338Z", | |
"shell.execute_reply": "2020-08-06T23:26:27.345Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"Done!\n" | |
] | |
} | |
], | |
"execution_count": 90, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T18:47:22.831Z", | |
"iopub.execute_input": "2020-08-06T18:47:22.840Z", | |
"iopub.status.idle": "2020-08-06T18:47:47.810Z", | |
"shell.execute_reply": "2020-08-06T18:47:47.789Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"### Serial comms parser" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"INBUF_SIZE = 64\n", | |
"MAX_CMD_VALUE = 6\n", | |
"\n", | |
"COMM_IDLE = 0\n", | |
"PKT_HEADER_MAGIC_2 = 1\n", | |
"PKT_HEADER_MAGIC_3 = 2\n", | |
"PKT_HEADER_PAYLOAD_LENGTH = 3\n", | |
"PKT_HEADER_CMD = 4\n", | |
"PKT_PAYLOAD = 5\n", | |
"PKT_CHECKSUM = 6\n", | |
"\n", | |
"state = COMM_IDLE\n", | |
"def receiveData(c):\n", | |
" global state\n", | |
" global checksum\n", | |
"\n", | |
" global payloadCounter\n", | |
" global payloadLength\n", | |
" global payloadBuffer\n", | |
" global commandTmp\n", | |
" \n", | |
" if state == COMM_IDLE:\n", | |
" if c == ord('$'):\n", | |
" state = PKT_HEADER_MAGIC_2\n", | |
" elif state == PKT_HEADER_MAGIC_2:\n", | |
" if c == ord('R'):\n", | |
" state = PKT_HEADER_MAGIC_3\n", | |
" else:\n", | |
" print(\"[UART]: Expected magic 'R', got \" + str(c))\n", | |
" state = COMM_IDLE\n", | |
" elif state == PKT_HEADER_MAGIC_3:\n", | |
" if c == ord('<'): \n", | |
" state = PKT_HEADER_PAYLOAD_LENGTH\n", | |
" \n", | |
" checksum = 0\n", | |
" else: \n", | |
" print(\"[UART]: Expected magic '<', got \" + str(c))\n", | |
" \n", | |
" state = COMM_IDLE\n", | |
" elif state == PKT_HEADER_PAYLOAD_LENGTH:\n", | |
" # Check that the buffer size is sane\n", | |
" if c > INBUF_SIZE:\n", | |
" print(\"[UART]: Declared payload size \" + str(c) + \", however the maximum accepted command value is \" + str(INBUF_SIZE))\n", | |
" state = COMM_IDLE\n", | |
" else:\n", | |
" payloadCounter = 0\n", | |
" payloadLength = c\n", | |
" payloadBuffer = [None] * payloadLength\n", | |
" \n", | |
" checksum = checksum ^ c\n", | |
" \n", | |
" state = PKT_HEADER_CMD\n", | |
" elif state == PKT_HEADER_CMD:\n", | |
" # Check that the command is sane\n", | |
" if c > MAX_CMD_VALUE:\n", | |
" print(\"[UART]: Received command \" + str(c) + \", however the maximum accepted command value is \" + str(MAX_CMD_VALUE))\n", | |
" state = COMM_IDLE\n", | |
" else: \n", | |
" checksum = checksum ^c\n", | |
" \n", | |
" commandTmp = c\n", | |
" \n", | |
" if payloadLength != 0:\n", | |
" state = PKT_PAYLOAD\n", | |
" else:\n", | |
" state = PKT_CHECKSUM\n", | |
" elif state == PKT_PAYLOAD:\n", | |
" payloadBuffer[payloadCounter] = c\n", | |
" payloadCounter = payloadCounter + 1\n", | |
"\n", | |
" checksum = checksum ^c\n", | |
" \n", | |
" if payloadCounter >= payloadLength:\n", | |
" state = PKT_CHECKSUM\n", | |
" elif state == PKT_CHECKSUM:\n", | |
" # This is the end of the road for this FSM, so because we have retursn in the `if:` statement go ahead and already set the state\n", | |
" state = COMM_IDLE\n", | |
" \n", | |
" if c != checksum:\n", | |
" print(\"[UART] CRC error, expected \" + str(c) + \", got \" + str(checksum))\n", | |
" else:\n", | |
" return commandTmp, payloadBuffer\n", | |
"\n", | |
"\n", | |
" # Default returned empty tuple\n", | |
" return None, None\n" | |
], | |
"outputs": [], | |
"execution_count": 55, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:28.496Z", | |
"iopub.execute_input": "2020-08-06T23:26:28.503Z", | |
"iopub.status.idle": "2020-08-06T23:26:28.516Z", | |
"shell.execute_reply": "2020-08-06T23:26:28.524Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"### Serial comms utilities" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"'''\n", | |
"Computes the checksum starting from the third byte of the packet\n", | |
"'''\n", | |
"def computeChecksum(packet):\n", | |
" crc = 0\n", | |
" \n", | |
" for i in range(3, len(packet)):\n", | |
" crc = crc ^ packet[i]\n", | |
" \n", | |
" return crc\n", | |
"\n", | |
"def generateHeader(payloadLength, payloadType):\n", | |
" # Initialize empty array\n", | |
" header = bytearray()\n", | |
"\n", | |
" header.append(ord('$'))\n", | |
" header.append(ord('R'))\n", | |
" header.append(ord('>'))\n", | |
" \n", | |
" header.append(payloadLength)\n", | |
" header.append(payloadType)\n", | |
" \n", | |
" return header\n", | |
" \n", | |
"'''\n", | |
"Takes a CFFI struct, extracts the buffer, and returns it as a bytearray\n", | |
"'''\n", | |
"def marshallPayload(payload):\n", | |
" \n", | |
" cffi_buffer = ffi.buffer(payload)\n", | |
"\n", | |
" # Initialize empty array\n", | |
" payload = bytearray(cffi_buffer)\n", | |
" \n", | |
" return payload\n", | |
"\n", | |
"def generateFooter(packet):\n", | |
" # initialize the footer\n", | |
" footer = bytearray()\n", | |
"\n", | |
" # Compute the checksum from the packet\n", | |
" checkSum = computeChecksum(packet)\n", | |
" \n", | |
" # Construct the footer\n", | |
" footer.append(checkSum)\n", | |
" \n", | |
" return footer\n", | |
"\n", | |
"'''\n", | |
" Transmits the board response for the 1780\n", | |
"'''\n", | |
"def transmitPollResponse(sensorData):\n", | |
"\n", | |
" payloadLength = ffi.sizeof(sensorData[0])\n", | |
" \n", | |
" # Generate the header\n", | |
" header = generateHeader(payloadLength, 5)\n", | |
" \n", | |
" # Marshall the payload\n", | |
" sensorPayload = marshallPayload(sensorData)\n", | |
" \n", | |
" # Assemble the partial packet\n", | |
" partialPacket = header + sensorPayload\n", | |
" \n", | |
" # Create the footer, using the partial packet\n", | |
" footer = generateFooter(partialPacket)\n", | |
" \n", | |
" # Assemble the full packet\n", | |
" packet = partialPacket + footer\n", | |
" \n", | |
" # Write the data to the serial port\n", | |
" serialPort.write(packet)\n", | |
"\n", | |
" return\n", | |
"\n", | |
"def transmitFirmwareResponse(firmwareData):\n", | |
" payloadLength = ffi.sizeof(firmwareData[0])\n", | |
" \n", | |
" # Generate the header\n", | |
" header = generateHeader(payloadLength, 1)\n", | |
" \n", | |
" # Marshall the payload\n", | |
" firmwarePayload = marshallPayload(firmwareData)\n", | |
" \n", | |
" # Assemble the partial packet\n", | |
" partialPacket = header + firmwarePayload\n", | |
" \n", | |
" # Create the footer, using the partial packet\n", | |
" footer = generateFooter(partialPacket)\n", | |
" \n", | |
" # Assemble the full packet\n", | |
" packet = partialPacket + footer\n", | |
" \n", | |
" # Write the data to the serial port\n", | |
" serialPort.write(packet)\n", | |
" \n", | |
"def transmitBoardResponse(boardData):\n", | |
" payloadLength = ffi.sizeof(boardData[0])\n", | |
" \n", | |
" # Generate the header\n", | |
" header = generateHeader(payloadLength, 2)\n", | |
" \n", | |
" # Marshall the payload\n", | |
" boardPayload = marshallPayload(boardData)\n", | |
" \n", | |
" # Assemble the partial packet\n", | |
" partialPacket = header + boardPayload\n", | |
" \n", | |
" # Create the footer, using the partial packet\n", | |
" footer = generateFooter(partialPacket)\n", | |
" \n", | |
" # Assemble the full packet\n", | |
" packet = partialPacket + footer\n", | |
" \n", | |
" # Write the data to the serial port\n", | |
" serialPort.write(packet)\n", | |
" \n", | |
" print(packet)" | |
], | |
"outputs": [], | |
"execution_count": 56, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:30.177Z", | |
"iopub.execute_input": "2020-08-06T23:26:30.185Z", | |
"iopub.status.idle": "2020-08-06T23:26:30.196Z", | |
"shell.execute_reply": "2020-08-06T23:26:30.203Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Flush the queue on the GUI\n", | |
"serialPort.write(b'\\r\\n')\n", | |
"\n", | |
"serialPort.write(b'Ready\\r\\n')\n", | |
"\n", | |
"#==============\n", | |
"time.sleep(0.1)\n", | |
"serialPort.write(b'reading the port\\r\\n')\n", | |
"print(serialPort.read(100))\n", | |
"\n", | |
"#==============\n", | |
"RCB_VERSION = 0\n", | |
"FIRMWARE_MAJOR = 2\n", | |
"FIRMWARE_MINOR = 10\n", | |
"\n", | |
"firmwareData.com_protocol_version = RCB_VERSION\n", | |
"firmwareData.major = FIRMWARE_MAJOR\n", | |
"firmwareData.minor = FIRMWARE_MINOR\n", | |
"\n", | |
"transmitFirmwareResponse(firmwareData)\n", | |
"\n", | |
"# =============================\n", | |
"time.sleep(0.5)\n", | |
"serialPort.write(b'reading the port\\r\\n')\n", | |
"print(serialPort.read(100))\n", | |
"\n", | |
"# =============================\n", | |
"\n", | |
"# version 0011 -> series 1780 v1\n", | |
"\n", | |
"boardData.version = 4 # Add + 1 to the version\n", | |
"boardData.id[0] = 0\n", | |
"boardData.id[1] = 222\n", | |
"boardData.id[2] = 173\n", | |
"boardData.id[3] = 0\n", | |
"boardData.id[4] = 190\n", | |
"boardData.id[5] = 239\n", | |
"boardData.id[6] = 0\n", | |
"boardData.id[7] = 0\n", | |
"boardData.tempProbeQty = 6\n", | |
"\n", | |
"boardData.s1780_PSA = True\n", | |
"boardData.s1780_LCA = True\n", | |
"boardData.s1780_PSB = True\n", | |
"boardData.s1780_LCB = True\n", | |
"boardData.s1780_PSA_BoardVersion = 4 # Set power-supply board (Side A) to v4 (which is highest power)\n", | |
"boardData.s1780_LCA_BoardVersion = 4 # Set load-cell board (Side A) to v4 (which is highest forces)\n", | |
"boardData.s1780_PSB_BoardVersion = 4 # Set power-supply board (Side A) to v4 (which is highest power)\n", | |
"boardData.s1780_LCB_BoardVersion = 4 # Set load-cell board (Side A) to v4 (which is highest forces)\n", | |
"\n", | |
"# Make up some arbitrary identifiers for the temperature probes\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[0])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x15\\x01\\x07\\x03'\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[1])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x05\\x22\\x07\\x04'\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[2])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x35\\x03\\x07\\x05'\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[3])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x05\\x44\\x07\\x06'\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[4])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x55\\x05\\x07\\x07'\n", | |
"D_buffer = ffi.buffer(boardData.tempProbes[5])\n", | |
"D_buffer[:] = b'\\x01\\x02\\x03\\x04\\x05\\x66\\x07\\x08'\n", | |
"transmitBoardResponse(boardData)\n", | |
"\n", | |
"#============================\n", | |
"time.sleep(0.5)\n", | |
"serialPort.write(b'reading the port\\r\\n')\n", | |
"print(serialPort.read(100))\n" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"b'$R<\\x00\\x01\\x01'\n", | |
"b'$R<\\x00\\x02\\x02'\n", | |
"bytearray(b'$R>@\\x02\\x04\\x00\\xde\\xad\\x00\\xbe\\xef\\x00\\x00\\x00\\x06\\x01\\x02\\x03\\x04\\x15\\x01\\x07\\x03\\x01\\x02\\x03\\x04\\x05\"\\x07\\x04\\x01\\x02\\x03\\x045\\x03\\x07\\x05\\x01\\x02\\x03\\x04\\x05D\\x07\\x06\\x01\\x02\\x03\\x04U\\x05\\x07\\x07\\x01\\x02\\x03\\x04\\x05f\\x07\\x08\\x0f\\x04\\x04\\x04\\x04\\x11')\n", | |
"b'$R<\\x00\\x00\\x00$R<\\t\\x05\\x00\\x00\\x00\\x00\\x00\\x00\\x00\\x00\\x00\\x0c'\n" | |
] | |
} | |
], | |
"execution_count": 58, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:52.423Z", | |
"iopub.execute_input": "2020-08-06T23:26:52.433Z", | |
"iopub.status.idle": "2020-08-06T23:26:53.511Z", | |
"shell.execute_reply": "2020-08-06T23:26:53.563Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"import random \n", | |
"\n", | |
"# //s1780 polled sensors\n", | |
"# typedef struct {\n", | |
"# float A_voltage; //in V\n", | |
"# float A_current; //in A\n", | |
"# float A_forces_N[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
"# //float A_brushless_hz; //motor brushless_hz\n", | |
"# float A_optical_hz; //motor optical_hz\n", | |
"# float tempC[TEMP_PROBES_QTY]; //temperature probes\n", | |
"# int16_t pressureP; //converted to the correct unit by the GUI\n", | |
"# int16_t pressureT; //converted to the correct unit by the GUI\n", | |
"# float B_voltage; //in V\n", | |
"# float B_current; //in A\n", | |
"# float B_forces_N[6]; //FX,FY,FZ,TX,TY,TZ\n", | |
"# //float A_brushless_hz; //motor brushless_hz\n", | |
"# float B_optical_hz; //motor optical_hz\n", | |
"# uint8_t overload : 1;\n", | |
"# uint8_t limitswitch : 1;\n", | |
"# } s1780_sensors_t;\n", | |
"\n", | |
"\n", | |
"for i in range(6):\n", | |
" sensorData.A_voltage = 10.23 + .5*random.random()\n", | |
" sensorData.B_voltage = 10.23 - .5*random.random()\n", | |
"\n", | |
" sensorData.A_current = 50.23 + .5*random.random()\n", | |
" sensorData.B_current = 50.23 - .5*random.random()\n", | |
"\n", | |
" sensorData.A_optical_hz = 30 + 1.5*random.random()\n", | |
" sensorData.B_optical_hz = 30 - 1.5*random.random()\n", | |
"\n", | |
" sensorData.pressureP = 1000+round(0*255*random.random())\n", | |
" sensorData.pressureT = 1000+round(0*255*random.random())\n", | |
"\n", | |
" sensorData.tempC[0] = 70.89 + 1*random.random()\n", | |
" sensorData.tempC[1] = 60.89 + 1*random.random()\n", | |
" sensorData.tempC[2] = 50.89 + 1*random.random()\n", | |
" sensorData.tempC[3] = 40.89 + 1*random.random()\n", | |
" sensorData.tempC[4] = 30.89 + 1*random.random()\n", | |
" sensorData.tempC[5] = 20.89 + 1*random.random()\n", | |
" \n", | |
" sensorData.overload = False\n", | |
" sensorData.limitswitch = False\n", | |
"\n", | |
" # Fx\n", | |
" sensorData.A_forces_N[0] = 0*1.1 + .05*random.random()\n", | |
" sensorData.B_forces_N[0] = 0*1.1 - .05*random.random()\n", | |
"\n", | |
" # Fy\n", | |
" sensorData.A_forces_N[1] = 0*1.5 + .05*random.random()\n", | |
" sensorData.B_forces_N[1] = 0*1.5 - .05*random.random()\n", | |
"\n", | |
" # Fz\n", | |
" sensorData.A_forces_N[2] = 0*10.9 + .05*random.random()\n", | |
" sensorData.B_forces_N[2] = 0*10.9 - .05*random.random()\n", | |
"\n", | |
" # Tx\n", | |
" sensorData.A_forces_N[3] = 1.0 + .0*random.random()\n", | |
" sensorData.B_forces_N[3] = 1.0 - .0*random.random()\n", | |
"\n", | |
" # Ty (thrust)\n", | |
" sensorData.A_forces_N[4] = 1 + .0*random.random()\n", | |
" sensorData.B_forces_N[4] = 1 - .0*random.random()\n", | |
"\n", | |
" # Tz (torque)\n", | |
" sensorData.A_forces_N[5] = 1.8 + .05*random.random()\n", | |
" sensorData.B_forces_N[5] = 1.8 - .05*random.random()\n", | |
"\n", | |
" transmitPollResponse(sensorData)\n", | |
" time.sleep(.1)\n", | |
" \n", | |
"# print(basicData.esc_voltage) ## Print \"1\"\n", | |
"\n", | |
"# D_buffer = ffi.buffer(basicData)\n", | |
"# print(binascii.hexlify((D_buffer)))\n", | |
"\n", | |
"# print(ffi.sizeof(basicData[0]) + ffi.sizeof(proData[0]))\n" | |
], | |
"outputs": [], | |
"execution_count": 59, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:56.166Z", | |
"iopub.execute_input": "2020-08-06T23:26:56.174Z", | |
"iopub.status.idle": "2020-08-06T23:26:56.736Z", | |
"shell.execute_reply": "2020-08-06T23:26:56.725Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Utilities" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"'''\n", | |
"Takes an input in [0,1] and maps it to the required duty cycle based on the servo PWM\n", | |
"scheme, which is 0 = 1000us and 1 = 2000us. Currently hardcoded for 48Mhz clock\n", | |
"''' \n", | |
"def input2dutycycle(throttleCmd):\n", | |
" pulseLength_us = throttleCmd * 1000 + 1000\n", | |
" pulseLength_ms = pulseLength_us / 1000\n", | |
"\n", | |
" timeEpoch_ms = 1/((TimerFrequency/2**16) / TimerClockDivisor) * 1000\n", | |
" timeStep_ms = timeEpoch_ms / 2**16\n", | |
"\n", | |
" # Calculate the number of timesteps and invert the on/off\n", | |
" dutyCycle = 65536 - math.ceil(pulseLength_ms / timeStep_ms)\n", | |
"\n", | |
" return dutyCycle\n", | |
"\n", | |
"'''\n", | |
"Output PWM throttle command. Currently hardcoded for Timer 0\n", | |
"'''\n", | |
"def setThrottle(throttleCmd):\n", | |
" # Saturate the throttle command\n", | |
" if throttleCmd < 0:\n", | |
" throttleCmd = 0\n", | |
" elif throttleCmd > 1:\n", | |
" throttleCmd = 1\n", | |
" \n", | |
" dutyCycle = input2dutycycle(throttleCmd)\n", | |
"\n", | |
" # Output to timer. Please note that the timer pin configuration happens elsewhere\n", | |
" d.getFeedback(u3.Timer0Config(TimerMode=0, Value=dutyCycle)) # U3, Timer 0" | |
], | |
"outputs": [], | |
"execution_count": 60, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:58.863Z", | |
"iopub.execute_input": "2020-08-06T23:26:58.871Z", | |
"iopub.status.idle": "2020-08-06T23:26:58.883Z", | |
"shell.execute_reply": "2020-08-06T23:26:58.889Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Initialize outputs" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"setThrottle(0)\n", | |
"# Configure LabJack IO ports\n", | |
"d.configIO(NumberOfTimersEnabled=1, TimerCounterPinOffset = 4, EnableCounter1=True)\n", | |
"\n", | |
"# Configure Timers\n", | |
"d.configTimerClock(TimerClockBase=TimerClockBase, TimerClockDivisor=TimerClockDivisor)" | |
], | |
"outputs": [ | |
{ | |
"output_type": "execute_result", | |
"execution_count": 61, | |
"data": { | |
"text/plain": [ | |
"{'TimerClockBase': 6, 'TimerClockDivisor': 2}" | |
] | |
}, | |
"metadata": {} | |
} | |
], | |
"execution_count": 61, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:26:59.939Z", | |
"iopub.execute_input": "2020-08-06T23:26:59.949Z", | |
"iopub.status.idle": "2020-08-06T23:26:59.964Z", | |
"shell.execute_reply": "2020-08-06T23:26:59.971Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Run the program" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"measurementComputing_queue = queue.Queue()\n", | |
"labJack_queue = queue.Queue()" | |
], | |
"outputs": [], | |
"execution_count": 62, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:27:03.166Z", | |
"iopub.execute_input": "2020-08-06T23:27:03.174Z", | |
"iopub.status.idle": "2020-08-06T23:27:03.185Z", | |
"shell.execute_reply": "2020-08-06T23:27:03.192Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"def measurementComputingSamplingTask(stop_event, timeStep):\n", | |
" global shouldPublishMeasurementComputingData\n", | |
" \n", | |
" now = time.time()\n", | |
" nextWakeTime = now + timeStep\n", | |
" \n", | |
" count = 0\n", | |
" \n", | |
" while not stop_event.is_set():\n", | |
" # Read the Measurement Computing device\n", | |
" temperatureReading = readTemperature(temperatureChannels)\n", | |
" \n", | |
" measurementComputing_queue.put(temperatureReading)\n", | |
" \n", | |
" shouldPublishMeasurementComputingData = True\n", | |
" \n", | |
"# print(temperatureReading)\n", | |
"\n", | |
" # Set up sleep until next loop\n", | |
" pause.until(nextWakeTime)\n", | |
" nextWakeTime = nextWakeTime + timeStep\n", | |
" count = count + 1\n", | |
" \n", | |
" return\n", | |
"\n", | |
"def labJackSamplingTask(stop_event, timeStep):\n", | |
" global shouldPublishLabJackData\n", | |
" \n", | |
" now = time.time()\n", | |
" nextWakeTime = now + timeStep\n", | |
" \n", | |
" count = 0\n", | |
" \n", | |
" while not stop_event.is_set():\n", | |
" # Measure analog input channels\n", | |
" adc0 = d.getAIN(0,SPECIAL_3V6_RANGE)\n", | |
" adc1 = d.getAIN(1,SPECIAL_3V6_RANGE)\n", | |
" adc2 = d.getAIN(2,SPECIAL_3V6_RANGE)\n", | |
" adc3 = d.getAIN(3,SPECIAL_3V6_RANGE)\n", | |
"\n", | |
" # Measure RPM\n", | |
" escTicks = d.getFeedback(u3.Counter1(Reset=True))[0]\n", | |
" motorRotations = escTicks / (numMagneticPoles/2)\n", | |
" motorSpeed_Hz = motorRotations / timeStep \n", | |
"\n", | |
"# print(\"escTicks :\" + str(escTicks) + \", timeStep :\" + str(timeStep) + \", motorSpeed_Hz :\" + str(motorSpeed_Hz))\n", | |
" \n", | |
" labJack_queue.put({'adc0': adc0, 'adc1': adc1, 'adc2': adc2, 'adc3': adc3, 'motorSpeed_Hz': motorSpeed_Hz})\n", | |
"\n", | |
" shouldPublishLabJackData = True\n", | |
" \n", | |
"# print(\"RPM: \" + str(rpm))\n", | |
" \n", | |
" # Set up sleep until next loop\n", | |
" pause.until(nextWakeTime)\n", | |
" nextWakeTime = nextWakeTime + timeStep\n", | |
" count = count + 1\n", | |
" \n", | |
" return\n", | |
" \n", | |
"\n", | |
"def incomingCommsTask(stop_event, timeStep):\n", | |
" global shouldSendDebugPacket\n", | |
" \n", | |
" now = time.time()\n", | |
" nextWakeTime = now + timeStep\n", | |
" \n", | |
" count = 0\n", | |
" \n", | |
" while not stop_event.is_set():\n", | |
" try:\n", | |
" while serialPort.in_waiting > 0:\n", | |
" # Fetch single byte\n", | |
" b = serialPort.read()\n", | |
"\n", | |
" # Transform byte into ordinal\n", | |
" c = ord(b)\n", | |
" command, payload = receiveData(c)\n", | |
"\n", | |
" if command != None:\n", | |
" processRCBenchmarkCommand(command, bytearray(payload))\n", | |
"\n", | |
" if command == 0:\n", | |
" shouldSendDebugPacket = True\n", | |
" if command == 5:\n", | |
"# transmitPollResponse(sensorData)\n", | |
" throttleCmd = (controlData.A_PWM -1000.0) / 1000\n", | |
" setThrottle(throttleCmd)\n", | |
" except Exception as e:\n", | |
" print(\"[COMM][ERROR]: \"+ str(e))\n", | |
" \n", | |
" # Set up sleep until next loop\n", | |
" pause.until(nextWakeTime)\n", | |
" nextWakeTime = nextWakeTime + timeStep\n", | |
" count = count + 1\n", | |
" \n", | |
" return\n", | |
"\n", | |
" \n", | |
"def outgoingCommsTask(stop_event, timeStep):\n", | |
" global shouldPublishLabJackData\n", | |
" global shouldPublishMeasurementComputingData\n", | |
" \n", | |
" now = time.time()\n", | |
" nextWakeTime = now + timeStep\n", | |
" \n", | |
" count = 0\n", | |
" \n", | |
" while not stop_event.is_set():\n", | |
" \n", | |
" shouldPublishData = False\n", | |
" \n", | |
" if labJack_queue.empty() == False:\n", | |
" val = labJack_queue.get()\n", | |
" \n", | |
" sensorData.A_voltage = val['adc0'] * motorAVoltageScaling\n", | |
" sensorData.B_voltage = float('nan') # 70.23 - .5*random.random()\n", | |
" \n", | |
" sensorData.A_current = val['adc1'] * motorACurrentScaling\n", | |
" sensorData.B_current = float('nan') # 50.23 - .5*random.random()\n", | |
"\n", | |
" sensorData.A_optical_hz = val['motorSpeed_Hz']\n", | |
" sensorData.B_optical_hz = float('nan') # 30 - .5*random.random()\n", | |
" \n", | |
" sensorData.pressureP = 1000+round(0*255*random.random())\n", | |
" sensorData.pressureT = 1000+round(0*255*random.random())\n", | |
" \n", | |
" sensorData.overload = False\n", | |
" sensorData.limitswitch = False\n", | |
"\n", | |
" # Fx\n", | |
" sensorData.A_forces_N[0] = float('nan') # 0*1.1 + .05*random.random()\n", | |
" sensorData.B_forces_N[0] = float('nan') # 0*1.1 - .05*random.random()\n", | |
"\n", | |
" # Fy\n", | |
" sensorData.A_forces_N[1] = float('nan') # 0*1.5 + .05*random.random()\n", | |
" sensorData.B_forces_N[1] = float('nan') # 0*1.5 - .05*random.random()\n", | |
"\n", | |
" # Fz\n", | |
" sensorData.A_forces_N[2] = float('nan') # 0*10.9 + .05*random.random()\n", | |
" sensorData.B_forces_N[2] = float('nan') # 0*10.9 - .05*random.random()\n", | |
"\n", | |
" # Tx\n", | |
" sensorData.A_forces_N[3] = float('nan') # 1.0 + .0*random.random()\n", | |
" sensorData.B_forces_N[3] = float('nan') # 1.0 - .0*random.random()\n", | |
"\n", | |
" # Ty (thrust)\n", | |
" sensorData.A_forces_N[4] = val['adc2'] * motorAThrustScaling\n", | |
" sensorData.B_forces_N[4] = float('nan') # 1 - .0*random.random()\n", | |
"\n", | |
" # Tz (torque)\n", | |
" sensorData.A_forces_N[5] = float('nan') # 1.8 + .05*random.random()\n", | |
" sensorData.B_forces_N[5] = float('nan') # 1.8 - .05*random.random()\n", | |
" \n", | |
" shouldPublishData = True\n", | |
" shouldPublishLabJackData = False\n", | |
" else:\n", | |
" sensorData.A_voltage = float('nan')\n", | |
" sensorData.B_voltage = float('nan')\n", | |
"\n", | |
" sensorData.A_current = float('nan')\n", | |
" sensorData.B_current = float('nan')\n", | |
"\n", | |
" sensorData.A_optical_hz = float('nan')\n", | |
" sensorData.B_optical_hz = float('nan')\n", | |
" \n", | |
" sensorData.pressureP = -2**15\n", | |
" sensorData.pressureT = -2**15\n", | |
"\n", | |
" sensorData.overload = False\n", | |
" sensorData.limitswitch = False\n", | |
"\n", | |
" # Fx\n", | |
" sensorData.A_forces_N[0] = float('nan')\n", | |
" sensorData.B_forces_N[0] = float('nan')\n", | |
"\n", | |
" # Fy\n", | |
" sensorData.A_forces_N[1] = float('nan')\n", | |
" sensorData.B_forces_N[1] = float('nan')\n", | |
"\n", | |
" # Fz\n", | |
" sensorData.A_forces_N[2] = float('nan')\n", | |
" sensorData.B_forces_N[2] = float('nan')\n", | |
"\n", | |
" # Tx\n", | |
" sensorData.A_forces_N[3] = float('nan')\n", | |
" sensorData.B_forces_N[3] = float('nan')\n", | |
"\n", | |
" # Ty (thrust)\n", | |
" sensorData.A_forces_N[4] = float('nan')\n", | |
" sensorData.B_forces_N[4] = float('nan')\n", | |
"\n", | |
" # Tz (torque)\n", | |
" sensorData.A_forces_N[5] = float('nan')\n", | |
" sensorData.B_forces_N[5] = float('nan')\n", | |
"\n", | |
" # Check if there is MeasurementComputing data queued to be published\n", | |
" if measurementComputing_queue.empty() == False:\n", | |
" reading = measurementComputing_queue.get()\n", | |
" \n", | |
" shouldPublishData = True\n", | |
" shouldPublishMeasurementComputingData = False\n", | |
" \n", | |
" for j in range(6):\n", | |
" if isinstance(reading[j], numbers.Number):\n", | |
" sensorData.tempC[j] = reading[j]\n", | |
" else:\n", | |
" sensorData.tempC[j] = float('nan')\n", | |
" else:\n", | |
" sensorData.tempC[0] = float('nan')\n", | |
" sensorData.tempC[1] = float('nan')\n", | |
" sensorData.tempC[2] = float('nan')\n", | |
" sensorData.tempC[3] = float('nan')\n", | |
" sensorData.tempC[4] = float('nan')\n", | |
" sensorData.tempC[5] = float('nan')\n", | |
"\n", | |
" if shouldPublishData == True:\n", | |
" transmitPollResponse(sensorData)\n", | |
" shouldPublishData = False\n", | |
" \n", | |
" # Set up sleep until next loop\n", | |
" pause.until(nextWakeTime)\n", | |
" nextWakeTime = nextWakeTime + timeStep\n", | |
" count = count + 1\n", | |
" \n", | |
" return" | |
], | |
"outputs": [], | |
"execution_count": 103, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-07T14:14:32.863Z", | |
"iopub.execute_input": "2020-08-07T14:14:32.872Z", | |
"iopub.status.idle": "2020-08-07T14:14:32.886Z", | |
"shell.execute_reply": "2020-08-07T14:14:32.896Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"shouldPublishLabJackData = False \n", | |
"shouldPublishMeasurementComputingData = False\n", | |
"shouldSendDebugPacket = False\n", | |
"\n", | |
"\n", | |
"# Create the kill switch. This will be used to direct all threads to \n", | |
"# exit their infinite loop.\n", | |
"pill2kill = threading.Event()\n", | |
"\n", | |
"# Instantiate the threads\n", | |
"measurementComputingSamplingTask_freqHz = 2 # Can only sample 2S/s/ch\n", | |
"labJackSamplingTask_freqHz = 4\n", | |
"incomingCommsTask_freqHz = 100\n", | |
"outgoingCommsTask_freqHz = 100\n", | |
"\n", | |
"mcThread = threading.Thread(target=measurementComputingSamplingTask, args=(pill2kill, 1.0/measurementComputingSamplingTask_freqHz))\n", | |
"ljThread = threading.Thread(target=labJackSamplingTask, args=(pill2kill, 1.0/labJackSamplingTask_freqHz))\n", | |
"incomingCommsThread = threading.Thread(target=incomingCommsTask, args=(pill2kill, 1.0/incomingCommsTask_freqHz))\n", | |
"outgoingCommsThread = threading.Thread(target=outgoingCommsTask, args=(pill2kill, 1.0/outgoingCommsTask_freqHz))\n", | |
"\n", | |
"# Flush the serial port\n", | |
"serialPort.flushInput()\n", | |
"\n", | |
"# Flush the queues\n", | |
"with measurementComputing_queue.mutex:\n", | |
" measurementComputing_queue.queue.clear()\n", | |
" \n", | |
"with labJack_queue.mutex:\n", | |
" labJack_queue.queue.clear()\n", | |
"\n", | |
"# Start the threads\n", | |
"mcThread.start()\n", | |
"ljThread.start()\n", | |
"incomingCommsThread.start()\n", | |
"outgoingCommsThread.start()\n", | |
"\n", | |
"print(\"Started at time: \" + str(datetime.datetime.now().time()))" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"Started at time: 10:14:33.664621\n" | |
] | |
} | |
], | |
"execution_count": 104, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-07T14:14:33.731Z", | |
"iopub.execute_input": "2020-08-07T14:14:33.745Z", | |
"iopub.status.idle": "2020-08-07T14:14:33.772Z", | |
"shell.execute_reply": "2020-08-07T14:14:33.782Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [], | |
"outputs": [ | |
{ | |
"output_type": "execute_result", | |
"execution_count": 86, | |
"data": { | |
"text/plain": [ | |
"1" | |
] | |
}, | |
"metadata": {} | |
} | |
], | |
"execution_count": 86, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T23:38:46.471Z", | |
"iopub.execute_input": "2020-08-06T23:38:46.480Z", | |
"iopub.status.idle": "2020-08-06T23:38:46.499Z", | |
"shell.execute_reply": "2020-08-06T23:38:46.507Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Stop program" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Send the kill signal\n", | |
"pill2kill.set()\n", | |
"\n", | |
"# Use join() to force the program to wait for the threads to end \n", | |
"mcThread.join()\n", | |
"ljThread.join()\n", | |
"incomingCommsThread.join()\n", | |
"outgoingCommsThread.join()\n", | |
"\n", | |
"print(\"Done at time: \" + str(datetime.datetime.now().time()))" | |
], | |
"outputs": [ | |
{ | |
"output_type": "stream", | |
"name": "stdout", | |
"text": [ | |
"Done at time: 22:10:58.316330\n" | |
] | |
} | |
], | |
"execution_count": 100, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-07T02:10:58.344Z", | |
"iopub.execute_input": "2020-08-07T02:10:58.355Z", | |
"iopub.status.idle": "2020-08-07T02:10:58.382Z", | |
"shell.execute_reply": "2020-08-07T02:10:58.427Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "markdown", | |
"source": [ | |
"# Shutdown program" | |
], | |
"metadata": { | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Close the Measurement Computing DAQ module\n", | |
"ul.release_daq_device(board_num)" | |
], | |
"outputs": [], | |
"execution_count": null, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-04T21:34:24.653Z", | |
"iopub.execute_input": "2020-08-04T21:34:24.658Z", | |
"iopub.status.idle": "2020-08-04T21:34:24.664Z", | |
"shell.execute_reply": "2020-08-04T21:34:24.669Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"# Close the LabJack\n", | |
"d.close()" | |
], | |
"outputs": [], | |
"execution_count": 30, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-06T01:01:26.835Z", | |
"iopub.execute_input": "2020-08-06T01:01:26.846Z", | |
"iopub.status.idle": "2020-08-06T01:01:26.856Z", | |
"shell.execute_reply": "2020-08-06T01:01:26.865Z" | |
} | |
} | |
}, | |
{ | |
"cell_type": "code", | |
"source": [ | |
"#Close the serial port\n", | |
"serialPort.close()" | |
], | |
"outputs": [], | |
"execution_count": null, | |
"metadata": { | |
"collapsed": true, | |
"jupyter": { | |
"source_hidden": false, | |
"outputs_hidden": false | |
}, | |
"nteract": { | |
"transient": { | |
"deleting": false | |
} | |
}, | |
"execution": { | |
"iopub.status.busy": "2020-08-05T16:39:30.200Z", | |
"iopub.execute_input": "2020-08-05T16:39:30.208Z", | |
"shell.execute_reply": "2020-08-05T16:39:30.225Z", | |
"iopub.status.idle": "2020-08-05T16:39:30.219Z" | |
} | |
} | |
} | |
], | |
"metadata": { | |
"kernel_info": { | |
"name": "python3" | |
}, | |
"language_info": { | |
"name": "python", | |
"version": "3.8.3", | |
"mimetype": "text/x-python", | |
"codemirror_mode": { | |
"name": "ipython", | |
"version": 3 | |
}, | |
"pygments_lexer": "ipython3", | |
"nbconvert_exporter": "python", | |
"file_extension": ".py" | |
}, | |
"kernelspec": { | |
"argv": [ | |
"python", | |
"-m", | |
"ipykernel_launcher", | |
"-f", | |
"{connection_file}" | |
], | |
"display_name": "Python 3", | |
"language": "python", | |
"name": "python3" | |
}, | |
"nteract": { | |
"version": "0.23.3" | |
}, | |
"gist_id": "4e895f9f58af44e06acd594b8d393e3b" | |
}, | |
"nbformat": 4, | |
"nbformat_minor": 0 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment