From 5ccea80929b8bddc37ba315d4a9a278e4c573f5c Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 29 Jun 2017 16:53:27 -0700 Subject: [PATCH] Fixed temp and voltage reading --- avr/src/config.h | 17 +++- avr/src/plan/buffer.c | 48 ++++----- avr/src/plan/exec.c | 6 ++ avr/src/plan/line.c | 5 +- avr/src/plan/planner.c | 4 +- avr/src/plan/velocity_curve.c | 4 + package.json | 2 +- scripts/install.sh | 32 +++++- scripts/update-bbctrl | 2 +- src/jade/templates/control-view.jade | 7 ++ src/pwr/Makefile | 2 +- src/pwr/main.c | 82 ++++++++------- src/py/bbctrl/AVR.py | 59 +++++------ src/py/bbctrl/Ctrl.py | 4 +- src/py/bbctrl/I2C.py | 57 +++++++++++ src/py/bbctrl/Jog.py | 6 ++ src/py/bbctrl/LCD.py | 144 +++++++++++++++------------ src/py/bbctrl/Pwr.py | 50 ++++++++++ src/py/bbctrl/__init__.py | 12 +-- src/py/inevent/JogHandler.py | 14 ++- src/py/lcd/__init__.py | 19 +--- src/stylus/style.styl | 8 +- 22 files changed, 380 insertions(+), 204 deletions(-) create mode 100644 src/py/bbctrl/I2C.py create mode 100644 src/py/bbctrl/Pwr.py diff --git a/avr/src/config.h b/avr/src/config.h index e71bf5a..17a3d2f 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -166,13 +166,21 @@ enum { // DRV8711 settings -#define DRV8711_OFF 24 +#if 0 // Doug's settings +#define DRV8711_OFF 48 +#define DRV8711_BLANK (0x80 | DRV8711_BLANK_ABT_bm) +#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_AUTO_OPT | 6) + +#else +#define DRV8711_OFF 12 #define DRV8711_BLANK (0x32 | DRV8711_BLANK_ABT_bm) #define DRV8711_DECAY (DRV8711_DECAY_DECMOD_MIXED | 16) +#endif + #define DRV8711_STALL (DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_4 | 200) #define DRV8711_DRIVE (DRV8711_DRIVE_IDRIVEP_50 | \ - DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 | \ - DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 | \ + DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_250 | \ + DRV8711_DRIVE_TDRIVEN_250 | DRV8711_DRIVE_OCPDEG_2 | \ DRV8711_DRIVE_OCPTH_500) #define DRV8711_TORQUE DRV8711_TORQUE_SMPLTH_200 #define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_450 | \ @@ -203,8 +211,7 @@ enum { // Planner /// Should be at least the number of buffers required to support optimal /// planning in the case of very short lines or arc segments. Suggest no less -/// than 12. Maximum is 255 with out also changing the type of mb.space. Must -/// leave headroom for stack. +/// than 12. Must leave headroom for stack. #define PLANNER_BUFFER_POOL_SIZE 32 /// Buffers to reserve in planner before processing new input line diff --git a/avr/src/plan/buffer.c b/avr/src/plan/buffer.c index 6bac8c1..74e4301 100644 --- a/avr/src/plan/buffer.c +++ b/avr/src/plan/buffer.c @@ -43,7 +43,7 @@ typedef struct { - uint8_t space; + uint16_t space; mp_buffer_t *tail; mp_buffer_t *head; mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE]; @@ -176,29 +176,29 @@ void mp_queue_dump() { void mp_buffer_print(const mp_buffer_t *bf) { - printf("{\n" - " ts: %d,\n" - " line: %d,\n" - " state: %d,\n" - " replannable: %s,\n" - " hold: %s,\n" - " value: %0.2f,\n" - " target: [%0.2f, %0.2f, %0.2f, %0.2f],\n" - " unit: [%0.2f, %0.2f, %0.2f, %0.2f],\n" - " length: %0.2f,\n" - " head_length: %0.2f,\n" - " body_length: %0.2f,\n" - " tail_length: %0.2f,\n" - " entry_velocity: %0.2f,\n" - " cruise_velocity: %0.2f,\n" - " exit_velocity: %0.2f,\n" - " braking_velocity: %0.2f,\n" - " entry_vmax: %0.2f,\n" - " cruise_vmax: %0.2f,\n" - " exit_vmax: %0.2f,\n" - " delta_vmax: %0.2f,\n" - " jerk: %0.2f,\n" - " cbrt_jerk: %0.2f\n" + printf("{" + "\"ts\":%d," + "\"line\":%d," + "\"state\":%d," + "\"replannable\":%s," + "\"hold\":%s," + "\"value\":%0.2f," + "\"target\":[%0.2f, %0.2f, %0.2f, %0.2f]," + "\"unit\":[%0.2f, %0.2f, %0.2f, %0.2f]," + "\"length\":%0.2f," + "\"head_length\":%0.2f," + "\"body_length\":%0.2f," + "\"tail_length\":%0.2f," + "\"entry_velocity\":%0.2f," + "\"cruise_velocity\":%0.2f," + "\"exit_velocity\":%0.2f," + "\"braking_velocity\":%0.2f," + "\"entry_vmax\":%0.2f," + "\"cruise_vmax\":%0.2f," + "\"exit_vmax\":%0.2f," + "\"delta_vmax\":%0.2f," + "\"jerk\":%0.2f," + "\"cbrt_jerk\":%0.2f" "}", bf->ts, bf->line, bf->state, bf->replannable ? "true" : "false", bf->hold ? "true" : "false", bf->value, bf->target[0], bf->target[1], bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2], diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index f46e96c..0b6da2b 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -253,6 +253,12 @@ static void _plan_hold() { /// Initializes move runtime with a new planner buffer static stat_t _exec_aline_init(mp_buffer_t *bf) { +#ifdef DEBUG + printf("buffer:"); + mp_buffer_print(bf); + putchar('\n'); +#endif + // Remove zero length lines. Short lines have already been removed. if (fp_ZERO(bf->length)) return STAT_NOOP; diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c index 6f17bbd..03c1ade 100644 --- a/avr/src/plan/line.c +++ b/avr/src/plan/line.c @@ -89,7 +89,7 @@ * * How to compute the radius using brute-force trig: * - * float theta = acos(costheta); + * float theta = acos(dot(a, b) / (norm(a) * norm(b))); * float radius = delta * sin(theta/2) / (1 - sin(theta/2)); * * This version extends Chamnit's algorithm by computing a value for delta that @@ -242,10 +242,11 @@ static void _calc_max_velocities(mp_buffer_t *bf, float move_time, bool exact_stop) { ASSERT(0 < move_time && isfinite(move_time)); + bf->cruise_vmax = bf->length / move_time; // target velocity requested + float junction_velocity = _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); - bf->cruise_vmax = bf->length / move_time; // target velocity requested bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax)); diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c index ac5149e..3a21889 100644 --- a/avr/src/plan/planner.c +++ b/avr/src/plan/planner.c @@ -275,9 +275,9 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { // B case: Velocities all match (or close enough) // This occurs frequently in normal gcode files with lots of short lines. // This case is not really necessary, but saves lots of processing time. - if (((bf->cruise_velocity - bf->entry_velocity) < + if ((fabs(bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) && - ((bf->cruise_velocity - bf->exit_velocity) < + (fabs(bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) { bf->body_length = bf->length; bf->head_length = 0; diff --git a/avr/src/plan/velocity_curve.c b/avr/src/plan/velocity_curve.c index e8db220..75c17fb 100644 --- a/avr/src/plan/velocity_curve.c +++ b/avr/src/plan/velocity_curve.c @@ -29,6 +29,8 @@ #include "velocity_curve.h" +#include + /// We are using a quintic (fifth-degree) Bezier polynomial for the velocity /// curve. This yields a constant pop; with pop being the sixth derivative @@ -62,6 +64,8 @@ /// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc /// v4.9.2 with -O3. float velocity_curve(float Vi, float Vt, float t) { + if (fabs(Vt - Vi) < 200) return Vi + (Vt - Vi) * t; + const float t2 = t * t; const float t3 = t2 * t; diff --git a/package.json b/package.json index c85da92..4740ce2 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "bbctrl", - "version": "0.1.9", + "version": "0.1.10", "homepage": "https://github.com/buildbotics/rpi-firmware", "license": "GPL 3+", diff --git a/scripts/install.sh b/scripts/install.sh index 274ce21..cdc912c 100755 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -1,7 +1,29 @@ #!/bin/bash -sudo service bbctrl stop -sudo ./scripts/avr109-flash.py avr/bbctrl-avr-firmware.hex -sudo rm -rf /usr/local/lib/python*/dist-packages/bbctrl-* -sudo ./setup.py install -sudo service bbctrl start +UPDATE_AVR=true +UPDATE_PY=true + +while [[ $# -gt 1 ]]; do + case "$1" in + --no-avr) UPDATE_AVR=false ;; + --no-py) UPDATE_PY=false ;; + esac + shift 1 +done + + +if $UPDATE_PY; then + if [ -e /var/run/bbctrl.pid ]; then + service bbctrl stop + fi +fi + +if $UPDATE_AVR; then + ./scripts/avr109-flash.py avr/bbctrl-avr-firmware.hex +fi + +if $UPDATE_PY; then + rm -rf /usr/local/lib/python*/dist-packages/bbctrl-* + ./setup.py install + service bbctrl start +fi diff --git a/scripts/update-bbctrl b/scripts/update-bbctrl index c9f2b5d..0930949 100644 --- a/scripts/update-bbctrl +++ b/scripts/update-bbctrl @@ -13,7 +13,7 @@ cd /tmp/update tar xf "$UPDATE" cd * -./scripts/install.sh +./scripts/install.sh "$*" cd - rm -rf /tmp/update diff --git a/src/jade/templates/control-view.jade b/src/jade/templates/control-view.jade index 4ed9f15..de9c2cd 100644 --- a/src/jade/templates/control-view.jade +++ b/src/jade/templates/control-view.jade @@ -117,6 +117,9 @@ script#control-view-template(type="text/x-template") input#tab4(type="radio", name="tabs") label(for="tab4") Console + input#tab5(type="radio", name="tabs") + label(for="tab5") Video + section#content1.tab-content .toolbar button.pure-button(title="Upload a new program file.", @click="open", @@ -180,3 +183,7 @@ script#control-view-template(type="text/x-template") td {{msg.code || '0'}} td {{msg.repeat}} td {{msg.msg}} + + section#content5.tab-content + .video + img.mjpeg(src="http://bbctrl.local:8000/stream/0") diff --git a/src/pwr/Makefile b/src/pwr/Makefile index 84c3e3e..69a69d3 100644 --- a/src/pwr/Makefile +++ b/src/pwr/Makefile @@ -1,7 +1,7 @@ # Makefile for the project Bulidbotics firmware PROJECT = bbctrl-pwr-firmware MCU = attiny1634 -CLOCK = 4000000 +CLOCK = 8000000 VERSION = 0.0.1 # Compile flags diff --git a/src/pwr/main.c b/src/pwr/main.c index a8f1ef7..836896e 100644 --- a/src/pwr/main.c +++ b/src/pwr/main.c @@ -48,9 +48,9 @@ #define VIN_PIN 0 #define VIN_ADC 5 -#define VOUT_PORT PORTB -#define VOUT_PIN 3 -#define VOUT_ADC 8 +#define VOUT_PORT PORTC +#define VOUT_PIN 2 +#define VOUT_ADC 11 #define GATE_PORT PORTC #define GATE_DDR DDRC @@ -75,23 +75,11 @@ typedef enum { CS1_REG, CS2_REG, CS3_REG, - TEMP_OFFSET_REG, - TEMP_GAIN_REG, NUM_REGS } regs_t; -uint16_t regs[NUM_REGS] = {0}; - - -float temp_offset; -float temp_gain; - - -uint8_t sig_row_read(uint16_t addr) { - SPMCSR = (1 << RSIG) | (1 << SPMEN); - return pgm_read_byte(addr); -} +volatile uint16_t regs[NUM_REGS] = {0}; void i2c_ack() {TWSCRB = (1 << TWCMD1) | (1 << TWCMD0);} @@ -99,8 +87,12 @@ void i2c_nack() {TWSCRB = (1 << TWAA) | (1 << TWCMD1) | (1 << TWCMD0);} ISR(TWI_SLAVE_vect) { - static uint8_t addr = 0; static uint8_t byte = 0; + static uint16_t reg; + + // Stretch clock longer to work around RPi bug + // See https://github.com/raspberrypi/linux/issues/254 + _delay_us(10); uint8_t status = TWSSRA; @@ -109,47 +101,59 @@ ISR(TWI_SLAVE_vect) { // send response if (byte < 2) { switch (byte++) { - case 0: TWSD = regs[addr]; break; - case 1: TWSD = regs[addr] >> 8; break; + case 0: TWSD = reg; break; + case 1: TWSD = reg >> 8; break; } i2c_ack(); } else i2c_nack(); - } else i2c_ack(); // Write + } else i2c_ack(); // Write ignore } else if (status & I2C_ADDRESS_STOP_INT_BM) { if (status & I2C_ADDRESS_MATCH_BM) { // read address - addr = (TWSD >> 1) & I2C_MASK; - byte = 0; + uint8_t addr = (TWSD >> 1) & I2C_MASK; + + if (addr < NUM_REGS) { + i2c_ack(); + reg = regs[addr]; + byte = 0; - if (addr < NUM_REGS) i2c_ack(); - else i2c_nack(); + } else i2c_nack(); } else TWSCRB = (1 << TWCMD1) | (0 << TWCMD0); // Stop } } +inline static uint16_t convert_voltage(uint16_t sample) { +#define VREF 1.1 +#define R1 34800 +#define R2 1000 + + return sample * (VREF / 1024.0 * (R1 + R2) / R2 * 100); +} + + void adc_conversion() { uint16_t data = ADC; uint8_t ch = ADMUX & 0xf; switch (ch) { case TEMP_ADC: - regs[TEMP_REG] = (temp_gain * data + temp_offset) * 100; + regs[TEMP_REG] = data; ch = VIN_ADC; break; case VIN_ADC: - regs[VIN_REG] = data * (1.1 / 1024.0 * 48100.0 / 1100.0 * 100); + regs[VIN_REG] = convert_voltage(data); ch = VOUT_ADC; break; case VOUT_ADC: - regs[VOUT_REG] = data * (1.1 / 1024.0 * 48100.0 / 1100.0 * 100); + regs[VOUT_REG] = convert_voltage(data); ch = CS1_ADC; break; @@ -185,11 +189,11 @@ ISR(ADC_vect) {adc_conversion();} void init() { cli(); - // CPU Clock, 4Mhz, disable CKOUT + // CPU Clock, disable CKOUT CCP = 0xd8; CLKSR = (1 << CSTR) | (1 << CKOUT_IO) | 0b0010; // 8Mhz internal clock CCP = 0xd8; - CLKPR = 0b0001; // div 2 + CLKPR = 0; // div 1 while (!((1 << 7) & CLKSR)) continue; // Wait for clock to stabilize // Power reduction @@ -198,34 +202,29 @@ void init() { // IO GATE_DDR = (1 << GATE1_PIN) | (1 << GATE2_PIN) | (1 << GATE3_PIN); // Out - PUEC |= 1 << 3; // Pull up reset line + PUEC = 1 << 3; // Pull up reset line // Disable digital IO on ADC lines DIDR0 = (1 << ADC3D) | (1 << ADC2D) | (1 << ADC0D); - DIDR1 = (1 << ADC5D) | (1 << ADC8D); + DIDR1 = (1 << ADC5D); + DIDR2 = (1 << ADC11D); - // ADC internal 1.1v, enable, with interrupt, prescale 32 + // ADC internal 1.1v, enable, with interrupt, prescale 128 ADMUX = (1 << REFS1) | (0 << REFS0); ADCSRA = (1 << ADEN) | (1 << ADIE) | - (1 << ADPS2) | (0 << ADPS1) | (1 << ADPS0); + (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); ADCSRB = 0; - // Timer (Clear output A on compare match, Fast PWM, diabled) + // Timer (Clear output A on compare match, Fast PWM, disabled) TCCR0A = (1 << COM0A1) | (0 << COM0A0) | (1 << WGM01) | (1 << WGM00); TCCR0B = 0 << WGM02; OCR0A = 255 * 0.2; // Initial duty cycle - // SPI, enable, smart mode, enable address/stop interrupt + // SPI, enable, enable address/stop interrupt TWSCRA = (1 << TWEN) | (1 << TWASIE) | (1 << TWDIE); TWSA = I2C_ADDR << 1; TWSAM = I2C_MASK << 1; - // Read calibration bytes - regs[TEMP_OFFSET_REG] = sig_row_read(0x2c); - regs[TEMP_GAIN_REG] = sig_row_read(0x2d); - temp_gain = sig_row_read(0x2c) / 128.0; - temp_offset = (int8_t)sig_row_read(0x2d); - sei(); } @@ -245,7 +244,6 @@ int main() { TCCR0A = TCCR0B = 0; GATE_DDR = (1 << GATE1_PIN) | (1 << 2); GATE_PORT = (1 << GATE1_PIN) | (1 << 2); - while (true) continue; // Start ADC adc_conversion(); diff --git a/src/py/bbctrl/AVR.py b/src/py/bbctrl/AVR.py index a238b53..85b3e39 100644 --- a/src/py/bbctrl/AVR.py +++ b/src/py/bbctrl/AVR.py @@ -5,11 +5,6 @@ import time import logging from collections import deque -try: - import smbus -except: - import smbus2 as smbus - import bbctrl @@ -44,6 +39,8 @@ class AVR(): self.queue = deque() self.in_buf = '' self.command = None + self.lcd_page = ctrl.lcd.add_new_page() + self.install_page = True try: self.sp = serial.Serial(ctrl.args.serial, ctrl.args.baud, @@ -58,13 +55,7 @@ class AVR(): ctrl.ioloop.add_handler(self.sp, self.serial_handler, ctrl.ioloop.READ) - try: - self.i2c_bus = smbus.SMBus(ctrl.args.avr_port) - self.i2c_addr = ctrl.args.avr_addr - - except FileNotFoundError as e: - self.i2c_bus = None - log.warning('Failed to open device: %s', e) + self.i2c_addr = ctrl.args.avr_addr def _start_sending_gcode(self, path): @@ -95,35 +86,24 @@ class AVR(): def _i2c_command(self, cmd, byte = None, word = None): - if self.i2c_bus is None or not hasattr(self.i2c_bus, 'write_byte'): - return - log.info('I2C: %d' % cmd) retry = 5 while True: try: - if byte is not None: - self.i2c_bus.write_byte_data(self.i2c_addr, cmd, byte) - - elif word is not None: - self.i2c_bus.write_word_data(self.i2c_addr, cmd, word) - - else: self.i2c_bus.write_byte(self.i2c_addr, cmd) + self.ctrl.i2c.write(self.i2c_addr, cmd, byte, word) break - except IOError as e: + except Exception as e: retry -= 1 if retry: - log.error('I2C communication failed, retrying: %s' % e) - self.i2c_bus.close() + log.error('AVR I2C communication failed, retrying: %s' % e) time.sleep(0.1) - self.i2c_bus = smbus.SMBus(self.ctrl.args.avr_port) continue else: - log.error('I2C communication failed: %s' % e) + log.error('AVR I2C communication failed: %s' % e) raise @@ -230,7 +210,12 @@ class AVR(): self.vars.update(update) try: - self.ctrl.lcd.update(update) + self._update_lcd(update) + + if self.install_page: + self.install_page = False + self.ctrl.lcd.set_current_page(self.lcd_page.id) + except Exception as e: log.error('Updating LCD: %s', e) @@ -240,6 +225,24 @@ class AVR(): log.error('Updating Web: %s', e) + def _update_lcd(self, msg): + if 'x' in msg or 'c' in msg: + v = self.ctrl.avr.vars + state = v.get('x', 'INIT') + if 'c' in v and state == 'RUNNING': state = v['c'] + + self.lcd_page.text('%-9s' % state, 0, 0) + + if 'xp' in msg: self.lcd_page.text('% 10.3fX' % msg['xp'], 9, 0) + if 'yp' in msg: self.lcd_page.text('% 10.3fY' % msg['yp'], 9, 1) + if 'zp' in msg: self.lcd_page.text('% 10.3fZ' % msg['zp'], 9, 2) + if 'ap' in msg: self.lcd_page.text('% 10.3fA' % msg['ap'], 9, 3) + if 't' in msg: self.lcd_page.text('%2uT' % msg['t'], 6, 1) + if 'u' in msg: self.lcd_page.text('%-6s' % msg['u'], 0, 1) + if 'f' in msg: self.lcd_page.text('%8uF' % msg['f'], 0, 2) + if 's' in msg: self.lcd_page.text('%8dS' % msg['s'], 0, 3) + + def queue_command(self, cmd): self.queue.append(cmd) self.set_write(True) diff --git a/src/py/bbctrl/Ctrl.py b/src/py/bbctrl/Ctrl.py index 6fa9fbb..27e6043 100644 --- a/src/py/bbctrl/Ctrl.py +++ b/src/py/bbctrl/Ctrl.py @@ -11,10 +11,12 @@ class Ctrl(object): self.args = args self.ioloop = ioloop + self.i2c = bbctrl.I2C(args.i2c_port) self.config = bbctrl.Config(self) + self.lcd = bbctrl.LCD(self) self.web = bbctrl.Web(self) self.avr = bbctrl.AVR(self) self.jog = bbctrl.Jog(self) - self.lcd = bbctrl.LCD(self) + self.pwr = bbctrl.Pwr(self) self.avr.connect() diff --git a/src/py/bbctrl/I2C.py b/src/py/bbctrl/I2C.py new file mode 100644 index 0000000..530e87e --- /dev/null +++ b/src/py/bbctrl/I2C.py @@ -0,0 +1,57 @@ +import logging + +try: + import smbus +except: + import smbus2 as smbus + +log = logging.getLogger('I2C') + + +class I2C(object): + def __init__(self, port): + self.port = port + self.i2c_bus = None + + + def connect(self): + if self.i2c_bus is None: + try: + self.i2c_bus = smbus.SMBus(self.port) + + except OSError as e: + self.i2c_bus = None + log.warning('Failed to open device: %s', e) + raise e + + + def read_word(self, addr): + self.connect() + + try: + return self.i2c_bus.read_word_data(addr, 0) + + except IOError as e: + log.warning('I2C read word failed: %s' % e) + self.i2c_bus.close() + self.i2c_bus = None + raise e + + + def write(self, addr, cmd, byte = None, word = None): + self.connect() + + try: + if byte is not None: + self.i2c_bus.write_byte_data(addr, cmd, byte) + + elif word is not None: + self.i2c_bus.write_word_data(addr, cmd, word) + + else: self.i2c_bus.write_byte(addr, cmd) + + except IOError as e: + log.warning('I2C write failed: %s' % e) + self.i2c_bus.close() + self.i2c_bus = None + raise e diff --git a/src/py/bbctrl/Jog.py b/src/py/bbctrl/Jog.py index 6f500a3..ac88ee4 100644 --- a/src/py/bbctrl/Jog.py +++ b/src/py/bbctrl/Jog.py @@ -25,6 +25,12 @@ class Jog(inevent.JogHandler): types = "js kbd".split()) + def up(self): self.ctrl.lcd.page_up() + def down(self): self.ctrl.lcd.page_down() + def left(self): self.ctrl.lcd.page_left() + def right(self): self.ctrl.lcd.page_right() + + def callback(self): if self.v != self.lastV: self.lastV = self.v diff --git a/src/py/bbctrl/LCD.py b/src/py/bbctrl/LCD.py index e3f015d..ba072db 100644 --- a/src/py/bbctrl/LCD.py +++ b/src/py/bbctrl/LCD.py @@ -7,6 +7,36 @@ import tornado.ioloop log = logging.getLogger('LCD') +class Page: + def __init__(self, lcd, text = None): + self.lcd = lcd + self.data = lcd.new_screen() + + if text is not None: + self.text(text, (lcd.width - len(text)) // 2, 1) + + + def put(self, c, x, y): + y += x // self.lcd.width + x %= self.lcd.width + y %= self.lcd.height + + if self.data[x][y] != c: + self.data[x][y] = c + if self == self.lcd.page: self.lcd.update() + + + def text(self, s, x, y): + for c in s: + self.put(c, x, y) + x += 1 + + def shift_left(self): pass + def shift_right(self): pass + def shift_up(self): pass + def shift_down(self): pass + + class LCD: def __init__(self, ctrl): self.ctrl = ctrl @@ -15,13 +45,13 @@ class LCD: self.height = 4 self.lcd = None self.timeout = None - self.clear_next_write = False self.reset = False + self.page = None + self.pages = [] + self.current_page = 0 + self.screen = self.new_screen() - self.clear() - self.text('Loading', 6, 1) - self.clear_next_write = True - self.update_screen() + self.set_message('Loading') # Redraw screen every 5 seconds self.redraw_timer = tornado.ioloop.PeriodicCallback(self._redraw, 5000, @@ -31,49 +61,63 @@ class LCD: atexit.register(self.goodbye) - def clear(self): - self.screen = [[[' ', False] for x in range(self.width)] - for y in range(self.height)] - self.redraw = True + def set_message(self, msg): + try: + self.load_page(Page(self, msg)) + self._update() + except IOError as e: + log.error('LCD communication failed: %s' % e) - def _trigger_update(self): - if self.timeout is None: - self.timeout = self.ctrl.ioloop.call_later(0.25, self.update_screen) + def new_screen(self): + return [[' ' for y in range(self.height)] for x in range(self.width)] - def _redraw(self): - self.redraw = True - self._trigger_update() + def new_page(self): return Page(self) + def add_page(self, page): self.pages.append(page) - def put(self, c, x, y): - if self.clear_next_write: - self.clear_next_write = False - self.clear() + def add_new_page(self): + page = self.new_page() + page.id = len(self.pages) + self.add_page(page) + return page - y += x // self.width - x %= self.width - y %= self.height - if self.screen[y][x][0] != c: - self.screen[y][x] = [c, True] - self._trigger_update() + def load_page(self, page): + if self.page != page: + self.page = page + self.redraw = True + self.update() - def text(self, s, x, y): - for c in s: - self.put(c, x, y) - x += 1 + def set_current_page(self, current_page): + self.current_page = current_page % len(self.pages) + self.load_page(self.pages[self.current_page]) + + + def page_up(self): pass + def page_down(self): pass + def page_right(self): self.set_current_page(self.current_page + 1) + def page_left(self): self.set_current_page(self.current_page - 1) + + + def update(self): + if self.timeout is None: + self.timeout = self.ctrl.ioloop.call_later(0.25, self._update) - def update_screen(self): + def _redraw(self): + self.redraw = True + self.update() + + + def _update(self): self.timeout = None try: if self.lcd is None: - self.lcd = lcd.LCD(self.ctrl.args.lcd_port, - self.ctrl.args.lcd_addr, + self.lcd = lcd.LCD(self.ctrl.i2c, self.ctrl.args.lcd_addr, self.height, self.width) if self.reset: @@ -85,41 +129,23 @@ class LCD: for y in range(self.height): for x in range(self.width): - cell = self.screen[y][x] + c = self.page.data[x][y] - if self.redraw or cell[1]: + if self.redraw or self.screen[x][y] != c: if cursorX != x or cursorY != y: self.lcd.goto(x, y) cursorX, cursorY = x, y - self.lcd.put_char(cell[0]) + self.lcd.put_char(c) cursorX += 1 - cell[1] = False + self.screen[x][y] = c self.redraw = False except IOError as e: log.error('LCD communication failed, retrying: %s' % e) self.reset = True - self.timeout = self.ctrl.ioloop.call_later(1, self.update_screen) - - - def update(self, msg): - if 'x' in msg or 'c' in msg: - v = self.ctrl.avr.vars - state = v.get('x', 'INIT') - if 'c' in v and state == 'RUNNING': state = v['c'] - - self.text('%-9s' % state, 0, 0) - - if 'xp' in msg: self.text('% 10.3fX' % msg['xp'], 9, 0) - if 'yp' in msg: self.text('% 10.3fY' % msg['yp'], 9, 1) - if 'zp' in msg: self.text('% 10.3fZ' % msg['zp'], 9, 2) - if 'ap' in msg: self.text('% 10.3fA' % msg['ap'], 9, 3) - if 't' in msg: self.text('%2uT' % msg['t'], 6, 1) - if 'u' in msg: self.text('%-6s' % msg['u'], 0, 1) - if 'f' in msg: self.text('%8uF' % msg['f'], 0, 2) - if 's' in msg: self.text('%8dS' % msg['s'], 0, 3) + self.timeout = self.ctrl.ioloop.call_later(1, self._update) def goodbye(self): @@ -131,10 +157,4 @@ class LCD: self.redraw_timer.stop() self.redraw_timer = None - if self.lcd is not None: - try: - self.lcd.clear() - self.lcd.display(1, 'Goodbye', lcd.JUSTIFY_CENTER) - - except IOError as e: - log.error('LCD communication failed: %s' % e) + if self.lcd is not None: self.set_message('Goodbye') diff --git a/src/py/bbctrl/Pwr.py b/src/py/bbctrl/Pwr.py new file mode 100644 index 0000000..c97da76 --- /dev/null +++ b/src/py/bbctrl/Pwr.py @@ -0,0 +1,50 @@ +import logging + +import bbctrl + +log = logging.getLogger('PWR') + + +# Must match regs in pwr firmare +TEMP_REG = 0 +VIN_REG = 1 +VOUT_REG = 2 +CS1_REG = 3 +CS2_REG = 4 +CS3_REG = 5 + + +class Pwr(): + def __init__(self, ctrl): + self.ctrl = ctrl + + self.i2c_addr = ctrl.args.pwr_addr + self.regs = [0] * 6 + self.lcd_page = ctrl.lcd.add_new_page() + + self._update() + + + def get_reg(self, i): return self.regs[i] + + + def _update(self): + try: + for i in range(len(self.regs)): + value = self.ctrl.i2c.read_word(self.i2c_addr + i) + + if i == TEMP_REG: self.regs[TEMP_REG] = value - 273 + elif i == VIN_REG: self.regs[VIN_REG] = value / 100.0 + elif i == VOUT_REG: self.regs[VOUT_REG] = value / 100.0 + else: self.regs[i] = value + + except Exception as e: + log.warning('Pwr communication failed: %s' % e) + self.ctrl.ioloop.call_later(1, self._update) + return + + self.lcd_page.text('%3dC' % self.regs[TEMP_REG], 0, 0) + self.lcd_page.text('%5.1fV in' % self.regs[VIN_REG], 0, 1) + self.lcd_page.text('%5.1fV out' % self.regs[VOUT_REG], 0, 2) + + self.ctrl.ioloop.call_later(0.25, self._update) diff --git a/src/py/bbctrl/__init__.py b/src/py/bbctrl/__init__.py index 25b6d6f..0b67bef 100755 --- a/src/py/bbctrl/__init__.py +++ b/src/py/bbctrl/__init__.py @@ -18,6 +18,8 @@ from bbctrl.AVR import AVR from bbctrl.Web import Web from bbctrl.Jog import Jog from bbctrl.Ctrl import Ctrl +from bbctrl.Pwr import Pwr +from bbctrl.I2C import I2C def get_resource(path): @@ -41,17 +43,13 @@ def parse_args(): help = 'Serial device') parser.add_argument('-b', '--baud', default = 115200, type = int, help = 'Serial baud rate') - parser.add_argument('--lcd-port', default = 1, type = int, - help = 'LCD I2C port') + parser.add_argument('--i2c-port', default = 1, type = int, + help = 'I2C port') parser.add_argument('--lcd-addr', default = 0x27, type = int, help = 'LCD I2C address') - parser.add_argument('--avr-port', default = 1, type = int, - help = 'AVR I2C port') parser.add_argument('--avr-addr', default = 0x2b, type = int, help = 'AVR I2C address') - parser.add_argument('--pwr-port', default = 1, type = int, - help = 'Power AVR I2C port') - parser.add_argument('--pwr-addr', default = 0x2c, type = int, + parser.add_argument('--pwr-addr', default = 0x60, type = int, help = 'Power AVR I2C address') parser.add_argument('-v', '--verbose', action = 'store_true', help = 'Verbose output') diff --git a/src/py/inevent/JogHandler.py b/src/py/inevent/JogHandler.py index 561a22f..62023e5 100644 --- a/src/py/inevent/JogHandler.py +++ b/src/py/inevent/JogHandler.py @@ -41,6 +41,12 @@ class JogHandler: log.debug(axes_to_string(self.axes) + ' x {:d}'.format(self.speed)) + def up(self): log.debug('up') + def down(self): log.debug('down') + def left(self): log.debug('left') + def right(self): log.debug('right') + + def __call__(self, event, state): if event.type not in [EV_ABS, EV_REL, EV_KEY]: return @@ -54,12 +60,12 @@ class JogHandler: axis = self.config['arrows'].index(event.code) if event.value < 0: - if axis == 1: log.debug('up') - else: log.debug('left') + if axis == 1: self.up() + else: self.left() elif 0 < event.value: - if axis == 1: log.debug('down') - else: log.debug('right') + if axis == 1: self.down() + else: self.right() elif event.type == EV_KEY and event.code in self.config['speed']: old_speed = self.speed diff --git a/src/py/lcd/__init__.py b/src/py/lcd/__init__.py index 5094eb9..f8065f5 100755 --- a/src/py/lcd/__init__.py +++ b/src/py/lcd/__init__.py @@ -1,10 +1,5 @@ #!/usr/bin/env python3 -try: - import smbus -except: - import smbus2 as smbus - import time import logging @@ -63,17 +58,11 @@ JUSTIFY_CENTER = 2 class LCD: - def __init__(self, port, addr, height = 4, width = 20): + def __init__(self, i2c, addr, height = 4, width = 20): self.addr = addr self.height = height self.width = width - - try: - self.bus = smbus.SMBus(port) - except FileNotFoundError as e: - self.bus = None - log.warning('Failed to open device: %s', e) - + self.i2c = i2c self.backlight = True self.reset() @@ -95,11 +84,9 @@ class LCD: def write_i2c(self, data): - if self.bus is None: return - if self.backlight: data |= BACKLIGHT_BIT - self.bus.write_byte(self.addr, data) + self.i2c.write(self.addr, data) time.sleep(0.0001) diff --git a/src/stylus/style.styl b/src/stylus/style.styl index 404a53f..757b7bb 100644 --- a/src/stylus/style.styl +++ b/src/stylus/style.styl @@ -290,8 +290,9 @@ body .tabs section - max-height 260px - overflow auto + max-height 610px + overflow-x hidden + overflow-y auto padding 0 margin 0 @@ -385,7 +386,8 @@ body > #tab1:checked ~ #content1, > #tab2:checked ~ #content2, > #tab3:checked ~ #content3, - > #tab4:checked ~ #content4 + > #tab4:checked ~ #content4, + > #tab5:checked ~ #content5 display block [id^="tab"]:checked + label -- 2.27.0