From 1bec606d7d90845a463846d87298c44408bbf8b2 Mon Sep 17 00:00:00 2001 From: zhaoshengEE Date: Sun, 22 Mar 2020 19:15:03 -0700 Subject: [PATCH 1/6] Vehicle interface with ultrasonic sensor --- vehicle_interface/RC_with_US/SoftI2CMaster.h | 862 +++++++++++++++++++ vehicle_interface/RC_with_US/UCMotor.cpp | 729 ++++++++++++++++ vehicle_interface/RC_with_US/UCMotor.h | 195 +++++ vehicle_interface/RC_with_US/main.ino | 143 +++ vehicle_interface/RC_with_US/pinout.h | 30 + vehicle_interface/RC_with_US/rc_commands.cpp | 67 ++ vehicle_interface/RC_with_US/rc_commands.h | 28 + vehicle_interface/RC_with_US/util.cpp | 14 + vehicle_interface/RC_with_US/util.h | 53 ++ vehicle_interface/RC_with_US/wheel.h | 51 ++ 10 files changed, 2172 insertions(+) create mode 100644 vehicle_interface/RC_with_US/SoftI2CMaster.h create mode 100644 vehicle_interface/RC_with_US/UCMotor.cpp create mode 100644 vehicle_interface/RC_with_US/UCMotor.h create mode 100644 vehicle_interface/RC_with_US/main.ino create mode 100644 vehicle_interface/RC_with_US/pinout.h create mode 100644 vehicle_interface/RC_with_US/rc_commands.cpp create mode 100644 vehicle_interface/RC_with_US/rc_commands.h create mode 100644 vehicle_interface/RC_with_US/util.cpp create mode 100644 vehicle_interface/RC_with_US/util.h create mode 100644 vehicle_interface/RC_with_US/wheel.h diff --git a/vehicle_interface/RC_with_US/SoftI2CMaster.h b/vehicle_interface/RC_with_US/SoftI2CMaster.h new file mode 100644 index 00000000..68de1a9a --- /dev/null +++ b/vehicle_interface/RC_with_US/SoftI2CMaster.h @@ -0,0 +1,862 @@ +/* Arduino SoftI2C library. + * + * Version 1.4 + * + * Copyright (C) 2013, Bernhard Nebel and Peter Fleury + * + * This is a very fast and very light-weight software I2C-master library + * written in assembler. It is based on Peter Fleury's I2C software + * library: http://homepage.hispeed.ch/peterfleury/avr-software.html + * Recently, the hardware implementation has been added to the code, + * which can be enabled by defining I2C_HARDWARE. + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino I2cMaster Library. If not, see + * . + */ + +/* In order to use the library, you need to define SDA_PIN, SCL_PIN, + * SDA_PORT and SCL_PORT before including this file. Have a look at + * http://www.arduino.cc/en/Reference/PortManipulation for finding out + * which values to use. For example, if you use digital pin 3 (corresponding + * to PD3) for SDA and digital pin 13 (corresponding to PB5) + * for SCL on a standard Arduino, + * you have to use the following definitions: + * #define SDA_PIN 3 + * #define SDA_PORT PORTD + * #define SCL_PIN 5 + * #define SCL_PORT PORTB + * + * Alternatively, you can define the compile time constant I2C_HARDWARE, + * in which case the TWI hardware is used. In this case you have to use + * the standard SDA/SCL pins (and, of course, the chip needs to support + * this). + * + * You can also define the following constants (see also below): + ' - I2C_PULLUP = 1 meaning that internal pullups should be used + * - I2C_CPUFREQ, when changing CPU clock frequency dynamically + * - I2C_FASTMODE = 1 meaning that the I2C bus allows speeds up to 400 kHz + * - I2C_SLOWMODE = 1 meaning that the I2C bus will allow only up to 25 kHz + * - I2C_NOINTERRUPT = 1 in order to prohibit interrupts while + * communicating (see below). This can be useful if you use the library + * for communicationg with SMbus devices, which have timeouts. + * Note, however, that interrupts are disabled from issuing a start condition + * until issuing a stop condition. So use this option with care! + * - I2C_TIMEOUT = 0..10000 msec in order to return from the I2C functions + * in case of a I2C bus lockup (i.e., SCL constantly low). 0 means no timeout. + * - I2C_MAXWAIT = 0..32767 number of retries in i2c_start_wait. 0 means never stop. + */ + +/* Changelog: + * Version 2.1 + * - added conditional to check whether it is the right MCU type + * Version 2.0 + * - added hardware support as well. + * Version 1.4: + * - added "maximum retry" in i2c_start_wait in order to avoid lockup + * - added "internal pullups", but be careful since the option stretches the I2C specs + * Version 1.3: + * - added "__attribute__ ((used))" for all functions declared with "__attribute__ ((noinline))" + * Now the module is also usable in Arduino 1.6.11+ + * Version 1.2: + * - added pragma to avoid "unused parameter warnings" (suggestion by Walter) + * - replaced wrong license file + * Version 1.1: + * - removed I2C_CLOCK_STRETCHING + * - added I2C_TIMEOUT time in msec (0..10000) until timeout or 0 if no timeout + * - changed i2c_init to return true iff both SDA and SCL are high + * - changed interrupt disabling so that the previous IRQ state is restored + * Version 1.0: basic functionality + */ + +#ifndef __AVR_ARCH__ +#error "Not an AVR MCU! Use 'SlowSoftI2CMaster' library instead of 'SoftI2CMaster'!" +#else + +#ifndef _SOFTI2C_H +#define _SOFTI2C_H 1 + +#include +#include +#include + +#pragma GCC diagnostic push + +#pragma GCC diagnostic ignored "-Wunused-parameter" + + + +// Init function. Needs to be called once in the beginning. +// Returns false if SDA or SCL are low, which probably means +// a I2C bus lockup or that the lines are not pulled up. +bool __attribute__ ((noinline)) i2c_init(void) __attribute__ ((used)); + +// Start transfer function: is the 8-bit I2C address (including the R/W +// bit). +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_start(uint8_t addr) __attribute__ ((used)); + +// Similar to start function, but wait for an ACK! Will timeout if I2C_MAXWAIT > 0. +bool __attribute__ ((noinline)) i2c_start_wait(uint8_t addr) __attribute__ ((used)); + +// Repeated start function: After having claimed the bus with a start condition, +// you can address another or the same chip again without an intervening +// stop condition. +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_rep_start(uint8_t addr) __attribute__ ((used)); + +// Issue a stop condition, freeing the bus. +void __attribute__ ((noinline)) i2c_stop(void) asm("ass_i2c_stop") __attribute__ ((used)); + +// Write one byte to the slave chip that had been addressed +// by the previous start call. is the byte to be sent. +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_write(uint8_t value) asm("ass_i2c_write") __attribute__ ((used)); + + +// Read one byte. If is true, we send a NAK after having received +// the byte in order to terminate the read sequence. +uint8_t __attribute__ ((noinline)) i2c_read(bool last) __attribute__ ((used)); + +// If you want to use the TWI hardeware, you have to define I2C_HARDWARE to be 1 +#ifndef I2C_HARDWARE +#define I2C_HARDWARE 0 +#endif + +#if I2C_HARDWARE +#ifndef TWDR +#error This chip does not support hardware I2C. Please undfine I2C_HARDWARE +#endif +#endif + +// You can set I2C_CPUFREQ independently of F_CPU if you +// change the CPU frequency on the fly. If you do not define it, +// it will use the value of F_CPU +#ifndef I2C_CPUFREQ +#define I2C_CPUFREQ F_CPU +#endif + +// If I2C_FASTMODE is set to 1, then the highest possible frequency below 400kHz +// is selected. Be aware that not all slave chips may be able to deal with that! +#ifndef I2C_FASTMODE +#define I2C_FASTMODE 0 +#endif + +// If I2C_FASTMODE is not defined or defined to be 0, then you can set +// I2C_SLOWMODE to 1. In this case, the I2C frequency will not be higher +// than 25KHz. This could be useful for problematic buses with high pull-ups +// and high capasitance. +#ifndef I2C_SLOWMODE +#define I2C_SLOWMODE 0 +#endif + +// If I2C_PULLUP is set to 1, then the internal pull-up resistors are used. +// This does not conform with the I2C specs, since the bus lines will be +// temporarily in high-state and the internal resistors have roughly 50k. +// With low bus speeds und short buses it usually works, though (hopefully). +#ifndef I2C_PULLUP +#define I2C_PULLUP 0 +#endif + +// if I2C_NOINTERRUPT is 1, then the I2C routines are not interruptable. +// This is most probably only necessary if you are using a 1MHz system clock, +// you are communicating with a SMBus device, and you want to avoid timeouts. +// Be aware that the interrupt bit is enabled after each call. So the +// I2C functions should not be called in interrupt routines or critical regions. +#ifndef I2C_NOINTERRUPT +#define I2C_NOINTERRUPT 0 +#endif + +// I2C_TIMEOUT can be set to a value between 1 and 10000. +// If it is defined and nonzero, it leads to a timeout if the +// SCL is low longer than I2C_TIMEOUT milliseconds, i.e., max timeout is 10 sec +#ifndef I2C_TIMEOUT +#define I2C_TIMEOUT 0 +#else +#if I2C_TIMEOUT > 10000 +#error I2C_TIMEOUT is too large +#endif +#endif + +// I2C_MAXWAIT can be set to any value between 0 and 32767. 0 means no time out. +#ifndef I2C_MAXWAIT +#define I2C_MAXWAIT 500 +#else +#if I2C_MAXWAIT > 32767 || I2C_MAXWAIT < 0 +#error Illegal I2C_MAXWAIT value +#endif +#endif + +#define I2C_TIMEOUT_DELAY_LOOPS (I2C_CPUFREQ/1000UL)*I2C_TIMEOUT/4000UL +#if I2C_TIMEOUT_DELAY_LOOPS < 1 +#define I2C_MAX_STRETCH 1 +#else +#if I2C_TIMEOUT_DELAY_LOOPS > 60000UL +#define I2C_MAX_STRETCH 60000UL +#else +#define I2C_MAX_STRETCH I2C_TIMEOUT_DELAY_LOOPS +#endif +#endif + +#if I2C_FASTMODE +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/350000L)/2-18)/3) +#define SCL_CLOCK 400000UL +#else +#if I2C_SLOWMODE +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/23500L)/2-18)/3) +#define SCL_CLOCK 25000UL +#else +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/90000L)/2-18)/3) +#define SCL_CLOCK 100000UL +#endif +#endif + +// constants for reading & writing +#define I2C_READ 1 +#define I2C_WRITE 0 + +#if !I2C_HARDWARE +// map the IO register back into the IO address space +#define SDA_DDR (_SFR_IO_ADDR(SDA_PORT) - 1) +#define SCL_DDR (_SFR_IO_ADDR(SCL_PORT) - 1) +#define SDA_OUT _SFR_IO_ADDR(SDA_PORT) +#define SCL_OUT _SFR_IO_ADDR(SCL_PORT) +#define SDA_IN (_SFR_IO_ADDR(SDA_PORT) - 2) +#define SCL_IN (_SFR_IO_ADDR(SCL_PORT) - 2) + +#ifndef __tmp_reg__ +#define __tmp_reg__ 0 +#endif + +// Internal delay functions. +void __attribute__ ((noinline)) i2c_delay_half(void) asm("ass_i2c_delay_half") __attribute__ ((used)); +void __attribute__ ((noinline)) i2c_wait_scl_high(void) asm("ass_i2c_wait_scl_high") __attribute__ ((used)); + +void i2c_delay_half(void) +{ // function call 3 cycles => 3C +#if I2C_DELAY_COUNTER < 1 + __asm__ __volatile__ (" ret"); + // 7 cycles for call and return +#else + __asm__ __volatile__ + ( + " ldi r25, %[DELAY] ;load delay constant ;; 4C \n\t" + "_Lidelay: \n\t" + " dec r25 ;decrement counter ;; 4C+xC \n\t" + " brne _Lidelay ;;5C+(x-1)2C+xC\n\t" + " ret ;; 9C+(x-1)2C+xC = 7C+xC" + : : [DELAY] "M" I2C_DELAY_COUNTER : "r25"); + // 7 cycles + 3 times x cycles +#endif +} + +void i2c_wait_scl_high(void) +{ +#if I2C_TIMEOUT <= 0 + __asm__ __volatile__ + ("_Li2c_wait_stretch: \n\t" + " sbis %[SCLIN],%[SCLPIN] ;wait for SCL high \n\t" + " rjmp _Li2c_wait_stretch \n\t" + " cln ;signal: no timeout \n\t" + " ret " + : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN)); +#else + __asm__ __volatile__ + ( " ldi r27, %[HISTRETCH] ;load delay counter \n\t" + " ldi r26, %[LOSTRETCH] \n\t" + "_Lwait_stretch: \n\t" + " clr __tmp_reg__ ;do next loop 255 times \n\t" + "_Lwait_stretch_inner_loop: \n\t" + " rcall _Lcheck_scl_level ;call check function ;; 12C \n\t" + " brpl _Lstretch_done ;done if N=0 ;; +1 = 13C\n\t" + " dec __tmp_reg__ ;dec inner loop counter;; +1 = 14C\n\t" + " brne _Lwait_stretch_inner_loop ;; +2 = 16C\n\t" + " sbiw r26,1 ;dec outer loop counter \n\t" + " brne _Lwait_stretch ;continue with outer loop \n\t" + " sen ;timeout -> set N-bit=1 \n\t" + " rjmp _Lwait_return ;and return with N=1\n\t" + "_Lstretch_done: ;SCL=1 sensed \n\t" + " cln ;OK -> clear N-bit \n\t" + " rjmp _Lwait_return ; and return with N=0 \n\t" + + "_Lcheck_scl_level: ;; call = 3C\n\t" + " cln ;; +1C = 4C \n\t" + " sbic %[SCLIN],%[SCLPIN] ;skip if SCL still low ;; +2C = 6C \n\t" + " rjmp _Lscl_high ;; +0C = 6C \n\t" + " sen ;; +1 = 7C\n\t " + "_Lscl_high: " + " nop ;; +1C = 8C \n\t" + " ret ;return N-Bit=1 if low ;; +4 = 12C\n\t" + + "_Lwait_return:" + : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), + [HISTRETCH] "M" (I2C_MAX_STRETCH>>8), + [LOSTRETCH] "M" (I2C_MAX_STRETCH&0xFF) + : "r26", "r27"); +#endif +} +#endif // !I2C_HARDWARE + +bool i2c_init(void) +#if I2C_HARDWARE +{ +#if I2C_PULLUP + digitalWrite(SDA, 1); + digitalWrite(SCL, 1); +#else + digitalWrite(SDA, 0); + digitalWrite(SCL, 0); +#endif +#if ((I2C_CPUFREQ/SCL_CLOCK)-16)/2 < 250 + TWSR = 0; /* no prescaler */ + TWBR = ((I2C_CPUFREQ/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ +#else + TWSR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_START) && (twst != TW_REP_START)) return false; + + // send device address + TWDR = addr; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return false; + + return true; +} +#else +{ + __asm__ __volatile__ + ( +#if I2C_NOINTERRUPT + " cli ;clear IRQ bit \n\t" +#endif + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;wait T/2 \n\t" + " rcall ass_i2c_write ;now write address \n\t" + " ret" + : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SCLIN] "I" (SCL_IN),[SCLPIN] "I" (SCL_PIN)); + return true; // we never return here! +} +#endif + +bool i2c_rep_start(uint8_t addr) +#if I2C_HARDWARE +{ + return i2c_start(addr); +} +#else +{ + __asm__ __volatile__ + + ( +#if I2C_NOINTERRUPT + " cli \n\t" +#endif +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable SDA pull-up\n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " rcall ass_i2c_write \n\t" + " ret" + : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), + [SCLIN] "I" (SCL_IN), [SCLOUT] "I" (SCL_OUT), [SDAOUT] "I" (SDA_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); + return true; // just to fool the compiler +} +#endif + +bool i2c_start_wait(uint8_t addr) +#if I2C_HARDWARE +{ + uint8_t twst; + uint16_t maxwait = I2C_MAXWAIT; +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + while (true) { + // send START condition + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_START) && (twst != TW_REP_START)) continue; + + // send device address + TWDR = addr; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) + { + /* device busy, send stop condition to terminate write operation */ + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + if (maxwait) + if (--maxwait == 0) + return false; + + continue; + } + //if( twst != TW_MT_SLA_ACK) return 1; + return true; + } +} +#else +{ + __asm__ __volatile__ + ( + " push r24 ;save original parameter \n\t" +#if I2C_MAXWAIT + " ldi r31, %[HIMAXWAIT] ;load max wait counter \n\t" + " ldi r30, %[LOMAXWAIT] ;load low byte \n\t" +#endif + "_Li2c_start_wait1: \n\t" + " pop r24 ;restore original parameter\n\t" + " push r24 ;and save again \n\t" +#if I2C_NOINTERRUPT + " cli ;disable interrupts \n\t" +#endif + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " rcall ass_i2c_write ;write address \n\t" + " tst r24 ;if device not busy -> done \n\t" + " brne _Li2c_start_wait_done \n\t" + " rcall ass_i2c_stop ;terminate write & enable IRQ \n\t" +#if I2C_MAXWAIT + " sbiw r30,1 ;decrement max wait counter\n\t" + " breq _Li2c_start_wait_done ;if zero reached, exit with false -> r24 already zero!\n\t" +#endif + " rjmp _Li2c_start_wait1 ;device busy, poll ack again \n\t" + "_Li2c_start_wait_done: \n\t" + " clr r25 ;clear high byte of return value\n\t" + " pop __tmp_reg__ ;pop off orig argument \n\t" + " ret " + : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAOUT] "I" (SDA_OUT), + [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), + [HIMAXWAIT] "M" (I2C_MAXWAIT>>8), + [LOMAXWAIT] "M" (I2C_MAXWAIT&0xFF) + : "r30", "r31" ); +} +#endif + +void i2c_stop(void) +#if I2C_HARDWARE +{ +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + /* send stop condition */ + TWCR = (1< I2C_TIMEOUT) return; +#endif + } +} +#else +{ + __asm__ __volatile__ + ( +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;T/2 delay \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;T/2 delay \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rcall ass_i2c_delay_half \n\t" +#if I2C_NOINTERRUPT + " sei ;enable interrupts again!\n\t" +#endif + : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); +} +#endif + + +bool i2c_write(uint8_t value) +#if I2C_HARDWARE +{ + uint8_t twst; +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + + // send data to the previously addressed device + TWDR = value; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits + twst = TW_STATUS & 0xF8; + if( twst != TW_MT_DATA_ACK) return false; + return true; +} +#else +{ + __asm__ __volatile__ + ( + " sec ;set carry flag \n\t" + " rol r24 ;shift in carry and shift out MSB \n\t" + " rjmp _Li2c_write_first \n\t" + "_Li2c_write_bit:\n\t" + " lsl r24 ;left shift into carry ;; 1C\n\t" + "_Li2c_write_first:\n\t" + " breq _Li2c_get_ack ;jump if TXreg is empty;; +1 = 2C \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 4C \n\t" + " nop \n\t" + " nop \n\t" + " nop \n\t" + " brcc _Li2c_write_low ;;+1/+2=5/6C\n\t" + " nop ;; +1 = 7C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA ;; +2 = 9C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rjmp _Li2c_write_high ;; +2 = 11C \n\t" + "_Li2c_write_low: \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; +2 = 9C \n\t" + " rjmp _Li2c_write_high ;;+2 = 11C \n\t" + "_Li2c_write_high: \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;;+X = 11C+X\n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;;+2 = 13C+X\n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " cln ;clear N-bit ;;+1 = 14C+X\n\t" + " nop \n\t" + " nop \n\t" + " nop \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for SCL high ;;+2 = 16C+X\n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brpl _Ldelay_scl_high ;;+2 = 18C+X\n\t" + "_Li2c_write_return_false: \n\t" + " clr r24 ; return false because of timeout \n\t" + " rjmp _Li2c_write_return \n\t" + "_Ldelay_scl_high: \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;;+X= 18C+2X\n\t" +#endif + " rjmp _Li2c_write_bit \n\t" + " ;; +2 = 20C +2X for one bit-loop \n\t" + "_Li2c_get_ack: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 5C \n\t" + " nop \n\t" + " nop \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA ;;+2 = 7C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; +X = 7C+X \n\t" +#endif + " clr r25 ;; 17C+2X \n\t" + " clr r24 ;return 0 ;; 14C + X \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; +2 = 9C+X\n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + "_Li2c_ack_wait: \n\t" + " cln ; clear N-bit ;; 10C + X\n\t" + " nop \n\t" + " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X \n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brmi _Li2c_write_return_false ;; 13C + X \n\t " + " sbis %[SDAIN],%[SDAPIN] ;if SDA hi -> return 0 ;; 15C + X \n\t" + " ldi r24,1 ;return true ;; 16C + X \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 16C + 2X \n\t" +#endif + "_Li2c_write_return: \n\t" + " nop \n\t " + " nop \n\t " +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" + " ret \n\t" + " ;; + 4 = 17C + 2X for acknowldge bit" + :: + [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN)); + return true; // fooling the compiler +} +#endif + +uint8_t i2c_read(bool last) +#if I2C_HARDWARE +{ +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + TWCR = (1< I2C_TIMEOUT) return 0xFF; +#endif + } + return TWDR; +} +#else +{ + __asm__ __volatile__ + ( + " ldi r23,0x01 \n\t" + "_Li2c_read_bit: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA(prev. ACK);; 4C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " nop \n\t" + " nop \n\t" + " nop \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 4C+X \n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 6C + X \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 6C + 2X \n\t" +#endif + " cln ; clear N-bit ;; 7C + 2X \n\t" + " nop \n\t " + " nop \n\t " + " nop \n\t " + " sbis %[SCLIN], %[SCLPIN] ;check for SCL high ;; 9C +2X \n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brmi _Li2c_read_return ;return if timeout ;; 10C + 2X\n\t" + " clc ;clear carry flag ;; 11C + 2X\n\t" + " sbic %[SDAIN],%[SDAPIN] ;if SDA is high ;; 11C + 2X\n\t" + " sec ;set carry flag ;; 12C + 2X\n\t" + " rol r23 ;store bit ;; 13C + 2X\n\t" + " brcc _Li2c_read_bit ;while receiv reg not full \n\t" + " ;; 15C + 2X for one bit loop \n\t" + + "_Li2c_put_ack: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" + " cpi r24,0 ;; 3C \n\t" + " breq _Li2c_put_ack_low ;if (ack=0) ;; 5C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rjmp _Li2c_put_ack_high \n\t" + "_Li2c_put_ack_low: ;else \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; 7C \n\t" + "_Li2c_put_ack_high: \n\t" + " nop \n\t " + " nop \n\t " + " nop \n\t " +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 7C + X \n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 9C +X \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " cln ;clear N ;; +1 = 10C\n\t" + " nop \n\t " + " nop \n\t " + " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X\n\t" + " rcall ass_i2c_wait_scl_high \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 11C + 2X\n\t" +#endif + "_Li2c_read_return: \n\t" + " nop \n\t " + " nop \n\t " +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + "sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" + " mov r24,r23 ;; 12C + 2X \n\t" + " clr r25 ;; 13 C + 2X\n\t" + " ret ;; 17C + X" + :: + [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN) + ); + return ' '; // fool the compiler! +} +#endif + +#pragma GCC diagnostic pop + +#endif +#endif diff --git a/vehicle_interface/RC_with_US/UCMotor.cpp b/vehicle_interface/RC_with_US/UCMotor.cpp new file mode 100644 index 00000000..d3e6501a --- /dev/null +++ b/vehicle_interface/RC_with_US/UCMotor.cpp @@ -0,0 +1,729 @@ +/** + * @file UCMotor.cpp + * @author + * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + * + * @note No edits have been made to to this file aside from the brief + * + */ + +// UCTRONOICS Motor shield library +// this code is public domain, enjoy! + + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #if defined(__AVR__) + #include + #endif + #include "WProgram.h" +#endif + +#include "UCMotor.h" + + + +static uint8_t latch_state; + +#if (MICROSTEPS == 8) +uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; +#elif (MICROSTEPS == 16) +uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; +#endif + +UCMotorController::UCMotorController(void) { + TimerInitalized = false; +} + +void UCMotorController::enable(void) { + // setup the latch + /* + LATCH_DDR |= _BV(LATCH); + ENABLE_DDR |= _BV(ENABLE); + CLK_DDR |= _BV(CLK); + SER_DDR |= _BV(SER); + */ + pinMode(MOTORLATCH, OUTPUT); + pinMode(MOTORENABLE, OUTPUT); + pinMode(MOTORDATA, OUTPUT); + pinMode(MOTORCLK, OUTPUT); + + latch_state = 0; + + latch_tx(); // "reset" + + //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! + digitalWrite(MOTORENABLE, LOW); +} + + +void UCMotorController::latch_tx(void) { + uint8_t i; + + //LATCH_PORT &= ~_BV(LATCH); + digitalWrite(MOTORLATCH, LOW); + + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + + for (i=0; i<8; i++) { + //CLK_PORT &= ~_BV(CLK); + digitalWrite(MOTORCLK, LOW); + + if (latch_state & _BV(7-i)) { + //SER_PORT |= _BV(SER); + digitalWrite(MOTORDATA, HIGH); + } else { + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + } + //CLK_PORT |= _BV(CLK); + digitalWrite(MOTORCLK, HIGH); + } + //LATCH_PORT |= _BV(LATCH); + digitalWrite(MOTORLATCH, HIGH); +} + +static UCMotorController MC; + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(9, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase + OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC4RS = 0x0000; + OC4R = 0x0000; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(10, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase + OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC5RS = 0x0000; + OC5R = 0x0000; + #else + // If we are not using PWM for pin 11, then just do digital + digitalWrite(11, LOW); + #endif +#else + #error "This chip is not supported!" +#endif + #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) + pinMode(11, OUTPUT); + //Serial .println("OK"); + #endif +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 + OC4RS = s; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 + OC5RS = s; + #else + // If we are not doing PWM output for M1, then just use on/off + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } + #endif +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase + OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC1RS = 0x0000; + OC1R = 0x0000; +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#elif defined(__PIC32MX__) + // Set the OC1 (pin3) PMW duty cycle from 0 to 255 + OC1RS = s; +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM3(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A / PD6 (pin 6) + TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A + //TCCR0B = freq & 0x7; + OCR0A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a + TCCR4B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR4A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase + OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC3RS = 0x0000; + OC3R = 0x0000; +#else + #error "This chip is not supported!" +#endif + pinMode(6, OUTPUT); +} + +inline void setPWM3(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR4A = s; +#elif defined(__PIC32MX__) + // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 + OC3RS = s; +#else + #error "This chip is not supported!" +#endif +} + + + +inline void initPWM4(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0B / PD5 (pin 5) + TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a + //TCCR0B = freq & 0x7; + OCR0B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 5 is now PE3 (OC3A) + TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a + TCCR3B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR3A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase + OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC2RS = 0x0000; + OC2R = 0x0000; +#else + #error "This chip is not supported!" +#endif + pinMode(5, OUTPUT); +} + +inline void setPWM4(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR3A = s; +#elif defined(__PIC32MX__) + // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 + OC2RS = s; +#else + #error "This chip is not supported!" +#endif +} + +UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + MC.enable(); + + switch (num) { + case 1: + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM1(freq); + break; + case 2: + latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM2(freq); + break; + case 3: + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM3(freq); + break; + case 4: + latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM4(freq); + break; + } +} + +void UC_DCMotor::run(uint8_t cmd) { + uint8_t a, b; + switch (motornum) { + case 1: + a = MOTOR1_A; b = MOTOR1_B; break; + case 2: + a = MOTOR2_A; b = MOTOR2_B; break; + case 3: + a = MOTOR3_A; b = MOTOR3_B; break; + case 4: + a = MOTOR4_A; b = MOTOR4_B; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + if(motornum ==3 || motornum ==4){ + latch_state |= _BV(a); + latch_state &= ~_BV(b); + }else{ + latch_state &= ~_BV(a); + latch_state |= _BV(b); + } + MC.latch_tx(); + break; + case BACKWARD: + if(motornum ==3 || motornum ==4){ + latch_state &= ~_BV(a); + latch_state |= _BV(b); + }else{ + latch_state |= _BV(a); + latch_state &= ~_BV(b); + } + MC.latch_tx(); + break; + case LEFT: + if(motornum ==3){ + a = MOTOR3_A; b = MOTOR3_B; //backward + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();} + else if(motornum ==4){ + a = MOTOR4_A; b = MOTOR4_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + }else if(motornum ==1){ + a = MOTOR1_A; b = MOTOR1_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + }else if(motornum ==2){ + a = MOTOR2_A; b = MOTOR2_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx(); + } + break; + case RIGHT: + if(motornum ==3){ + a = MOTOR3_A; b = MOTOR3_B; //backward + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx();} + else if(motornum ==4){ + a = MOTOR4_A; b = MOTOR4_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();}else if(motornum ==1){ + a = MOTOR1_A; b = MOTOR1_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();}else if(motornum ==2){ + a = MOTOR2_A; b = MOTOR2_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + } + break; + case STOP: + latch_state &= ~_BV(a); // A and B both low + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + } +} + +void UC_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + case 3: + setPWM3(speed); break; + case 4: + setPWM4(speed); break; + } +} + +/****************************************** + STEPPERS +******************************************/ + +UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { + MC.enable(); + + revsteps = steps; + steppernum = num; + currentstep = 0; + + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(11, OUTPUT); + pinMode(3, OUTPUT); + digitalWrite(11, HIGH); + digitalWrite(3, HIGH); + + // use PWM for microstepping support + initPWM1(STEPPER1_PWM_RATE); + initPWM2(STEPPER1_PWM_RATE); + setPWM1(255); + setPWM2(255); + + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(5, OUTPUT); + pinMode(6, OUTPUT); + digitalWrite(5, HIGH); + digitalWrite(6, HIGH); + + // use PWM for microstepping support + // use PWM for microstepping support + initPWM3(STEPPER2_PWM_RATE); + initPWM4(STEPPER2_PWM_RATE); + setPWM3(255); + setPWM4(255); + } +} + +void UC_Stepper::setSpeed(uint16_t rpm) { + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); + steppingcounter = 0; +} + +void UC_Stepper::release(void) { + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + } +} + +void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { + uint32_t uspers = usperstep; + uint8_t ret = 0; + + if (style == INTERLEAVE) { + uspers /= 2; + } + else if (style == MICROSTEP) { + uspers /= MICROSTEPS; + steps *= MICROSTEPS; +#ifdef MOTORDEBUG + Serial.print("steps = "); Serial.println(steps, DEC); +#endif + } + + while (steps--) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + if (style == MICROSTEP) { + while ((ret != 0) && (ret != MICROSTEPS)) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + } +} + +uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { + uint8_t a, b, c, d; + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + if (steppernum == 1) { + a = _BV(MOTOR1_A); + b = _BV(MOTOR2_A); + c = _BV(MOTOR1_B); + d = _BV(MOTOR2_B); + } else if (steppernum == 2) { + a = _BV(MOTOR3_A); + b = _BV(MOTOR4_A); + c = _BV(MOTOR3_B); + d = _BV(MOTOR4_B); + } else { + return 0; + } + + // next determine what sort of stepping procedure we're up to + if (style == SINGLE) { + if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } + else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next even step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } + else { + currentstep -= MICROSTEPS; + } + } + } else if (style == DOUBLE) { + if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next odd step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == INTERLEAVE) { + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } + + if (style == MICROSTEP) { + if (dir == FORWARD) { + currentstep++; + } else { + // BACKWARDS + currentstep--; + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + + ocra = ocrb = 0; + if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { + ocra = microstepcurve[MICROSTEPS - currentstep]; + ocrb = microstepcurve[currentstep]; + } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { + ocra = microstepcurve[currentstep - MICROSTEPS]; + ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; + } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { + ocra = microstepcurve[MICROSTEPS*3 - currentstep]; + ocrb = microstepcurve[currentstep - MICROSTEPS*2]; + } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { + ocra = microstepcurve[currentstep - MICROSTEPS*3]; + ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; + } + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + +#ifdef MOTORDEBUG + Serial.print("current step: "); Serial.println(currentstep, DEC); + Serial.print(" pwmA = "); Serial.print(ocra, DEC); + Serial.print(" pwmB = "); Serial.println(ocrb, DEC); +#endif + + if (steppernum == 1) { + setPWM1(ocra); + setPWM2(ocrb); + } else if (steppernum == 2) { + setPWM3(ocra); + setPWM4(ocrb); + } + + + // release all + latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 + + //Serial.println(step, DEC); + if (style == MICROSTEP) { + if ((currentstep >= 0) && (currentstep < MICROSTEPS)) + latch_state |= a | b; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) + latch_state |= b | c; + if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) + latch_state |= c | d; + if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) + latch_state |= d | a; + } else { + switch (currentstep/(MICROSTEPS/2)) { + case 0: + latch_state |= a; // energize coil 1 only + break; + case 1: + latch_state |= a | b; // energize coil 1+2 + break; + case 2: + latch_state |= b; // energize coil 2 only + break; + case 3: + latch_state |= b | c; // energize coil 2+3 + break; + case 4: + latch_state |= c; // energize coil 3 only + break; + case 5: + latch_state |= c | d; // energize coil 3+4 + break; + case 6: + latch_state |= d; // energize coil 4 only + break; + case 7: + latch_state |= d | a; // energize coil 1+4 + break; + } + } + + + MC.latch_tx(); + return currentstep; +} + diff --git a/vehicle_interface/RC_with_US/UCMotor.h b/vehicle_interface/RC_with_US/UCMotor.h new file mode 100644 index 00000000..1e2f1e05 --- /dev/null +++ b/vehicle_interface/RC_with_US/UCMotor.h @@ -0,0 +1,195 @@ +/** + * @file UCMotor.h + * @author + * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + * + * @note No edits have been made to to this file aside from the brief + * + */ +// UCTRONICS Motor shield library +// this code is public domain, enjoy! + +/* + * Usage Notes: + * For PIC32, all features work properly with the following two exceptions: + * + * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 + * to completely operate (four for motor outputs and two for RC servos), the + * M1 motor output will not have PWM ability when used with a PIC32 board. + * However, there is a very simple workaround. If you need to drive a stepper + * or DC motor with PWM on motor output M1, you can use the PWM output on pin + * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for + * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting + * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the + * two #defines below to activate the PWM on either pin 9 or pin 10. You will + * then have a fully functional microstepping for 2 stepper motors, or four + * DC motor outputs with PWM. + * + * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and + * the operation of DC motors and stepper motors as of 9/2012. This issue + * will get fixed in future MPIDE releases, but at the present time it means + * that the Motor Party example will NOT work properly. Any time you attach + * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will + * stop working. Thus no steppers or DC motors. + * + */ +// 09/15/2012 Modified for use with chipKIT boards + + +#ifndef _UCMotor_h_ +#define _UCMotor_h_ + +#include +#if defined(__AVR__) + #include + + //#define MOTORDEBUG 1 + + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ _BV(CS20) // no prescale + #define MOTOR12_8KHZ _BV(CS21) // divide by 8 + #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 + #define MOTOR12_1KHZ _BV(CS22) // divide by 64 + + #define MOTOR34_64KHZ _BV(CS00) // no prescale + #define MOTOR34_8KHZ _BV(CS01) // divide by 8 + #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 + +#elif defined(__PIC32MX__) + //#define MOTORDEBUG 1 + + // Uncomment the one of following lines if you have put a jumper from + // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. + // Either will enable PWM for M1 + //#define PIC32_USE_PIN9_FOR_M1_PWM + //#define PIC32_USE_PIN10_FOR_M1_PWM + + #define MICROSTEPS 16 // 8 or 16 + + // For PIC32 Timers, define prescale settings by PWM frequency + #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz + + #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz + + // PWM rate for DC motors. + #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ + // Note: for PIC32, both of these must be set to the same value + // since there's only one timebase for all 4 PWM outputs + #define STEPPER1_PWM_RATE MOTOR12_39KHZ + #define STEPPER2_PWM_RATE MOTOR34_39KHZ + +#endif + +// Bit positions in the 74HCT595 shift register output +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +// Constants that the user passes in to the motor calls +#define FORWARD 1 +#define BACKWARD 2 +#define LEFT 3 +#define RIGHT 4 +#define STOP 5 + +#define FORWARD2 2 +#define BACKWARD2 1 + +#define BRAKE 3 +#define RELEASE 4 + +// Constants that the user passes in to the stepper calls +#define SINGLE 1 +#define DOUBLE 2 +#define INTERLEAVE 3 +#define MICROSTEP 4 + +/* +#define LATCH 4 +#define LATCH_DDR DDRB +#define LATCH_PORT PORTB + +#define CLK_PORT PORTD +#define CLK_DDR DDRD +#define CLK 4 + +#define ENABLE_PORT PORTD +#define ENABLE_DDR DDRD +#define ENABLE 7 + +#define SER 0 +#define SER_DDR DDRB +#define SER_PORT PORTB +*/ + +// Arduino pin names for interface to 74HCT595 latch +#define MOTORLATCH 12 +#define MOTORCLK 4 +#define MOTORENABLE 7 +#define MOTORDATA 8 + +class UCMotorController +{ + public: + UCMotorController(void); + void enable(void); + friend class UC_DCMotor; + void latch_tx(void); + uint8_t TimerInitalized; +}; + +class UC_DCMotor +{ + public: + UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +class UC_Stepper { + public: + UC_Stepper(uint16_t, uint8_t); + void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); + void setSpeed(uint16_t); + uint8_t onestep(uint8_t dir, uint8_t style); + void release(void); + uint16_t revsteps; // # steps per revolution + uint8_t steppernum; + uint32_t usperstep, steppingcounter; + private: + uint8_t currentstep; + +}; + +uint8_t getlatchstate(void); + +#endif diff --git a/vehicle_interface/RC_with_US/main.ino b/vehicle_interface/RC_with_US/main.ino new file mode 100644 index 00000000..1e80305d --- /dev/null +++ b/vehicle_interface/RC_with_US/main.ino @@ -0,0 +1,143 @@ +#include "pinout.h" +#include "rc_commands.h" +#include "UCMotor.h" +#include +#include +#include //You will need to install this library + +volatile Array commands = all_brake_command; + +// Initialize the UC motor objects +UC_DCMotor mL(1); +UC_DCMotor mR(2); +UC_DCMotor * minis[2] = { + &mL, + &mR +}; + +void setup() +{ + // For debugging + Serial.begin(9600); + Serial.println("Init"); + + // Setup RC pins in input mode (input capture) + pinMode(RC_RIGHT_CHANNEL_PIN, INPUT); + pinMode(RC_LEFT_CHANNEL_PIN, INPUT); + pinMode(RC_SWA_CHANNEL_PIN, INPUT); + pinMode(RC_SWB_CHANNEL_PIN, INPUT); + + //Initialize I2C protocol for ultrasonic sensor + i2c_init(); + + // Indicate setup done + Serial.println("Setup complete"); +} + +/** + * @brief Sends commands to a UC_DC motor + * + * @param uc_motor reference to the UC_DC motor object + * @param command the command to send + * + * @return + */ +void sendCommandMini(UC_DCMotor * uc_motor, wheel_motor_command_t command) { + // BRAKE condition + if (command.brake_release == ENGAGE_BRAKE) { + uc_motor->run(STOP); + uc_motor->setSpeed(0); + return; + } + + // Step 1: set rotation direction + String debug_str = String(); + if (command.dir == DIR_FW) { + uc_motor->run(FORWARD); + } else { + uc_motor->run(BACKWARD); + } + + // Step 2: set speed + uc_motor->setSpeed(command.duty_cycle * TOP); +} + +void loop() +{ + int distance = read_the_sensor_example(); + if (distance > 25){ + // Update wheel commands + commands = fetch_rc_commands(); + } + + else{ + commands = all_brake_command; + } + // Send commands to wheels + for (int i = wheel_indices::Start + 1; i < wheel_indices::End; i++ ) { + sendCommandMini(minis[i], commands[i]); + } +} + +////////////////////////////////////////////////////////// +// Code Example: Read the sensor at the default address // +////////////////////////////////////////////////////////// +int read_the_sensor_example(){ + boolean error = 0; //Create a bit to check for catch errors as needed. + int distance; + + //Take a range reading at the default address of 224 + error = start_sensor(224); //Start the sensor and collect any error codes. + if (!error){ //If you had an error starting the sensor there is little point in reading it as you will get old data. + delay(100); + distance = read_sensor(224); //reading the sensor will return an integer value -- if this value is 0 there was an error + //Serial.print("Distance:");Serial.println(range);Serial.print("cm"); + Serial.println((String)"Distance:"+distance+" cm"); + return distance; + } +} + +/////////////////////////////////////////////////// +// Function: Start a range reading on the sensor // +/////////////////////////////////////////////////// +//Uses the I2C library to start a sensor at the given address +//Collects and reports an error bit where: 1 = there was an error or 0 = there was no error. +//INPUTS: byte bit8address = the address of the sensor that we want to command a range reading +//OUPUTS: bit errorlevel = reports if the function was successful in taking a range reading: 1 = the function +// had an error, 0 = the function was successful +boolean start_sensor(byte bit8address){ + boolean errorlevel = 0; + bit8address = bit8address & B11111110; //Do a bitwise 'and' operation to force the last bit to be zero -- we are writing to the address. + errorlevel = !i2c_start(bit8address) | errorlevel; //Run i2c_start(address) while doing so, collect any errors where 1 = there was an error. + errorlevel = !i2c_write(81) | errorlevel; //Send the 'take range reading' command. (notice how the library has error = 0 so I had to use "!" (not) to invert the error) + i2c_stop(); + return errorlevel; +} + + + +/////////////////////////////////////////////////////////////////////// +// Function: Read the range from the sensor at the specified address // +/////////////////////////////////////////////////////////////////////// +//Uses the I2C library to read a sensor at the given address +//Collects errors and reports an invalid range of "0" if there was a problem. +//INPUTS: byte bit8address = the address of the sensor to read from +//OUPUTS: int range = the distance in cm that the sensor reported; if "0" there was a communication error +int read_sensor(byte bit8address){ + boolean errorlevel = 0; + int range = 0; + byte range_highbyte = 0; + byte range_lowbyte = 0; + bit8address = bit8address | B00000001; //Do a bitwise 'or' operation to force the last bit to be 'one' -- we are reading from the address. + errorlevel = !i2c_start(bit8address) | errorlevel; + range_highbyte = i2c_read(0); //Read a byte and send an ACK (acknowledge) + range_lowbyte = i2c_read(1); //Read a byte and send a NACK to terminate the transmission + i2c_stop(); + range = (range_highbyte * 256) + range_lowbyte; //compile the range integer from the two bytes received. + if(errorlevel){ + return 0; + } + else{ + return range; + } +} diff --git a/vehicle_interface/RC_with_US/pinout.h b/vehicle_interface/RC_with_US/pinout.h new file mode 100644 index 00000000..94d3be7a --- /dev/null +++ b/vehicle_interface/RC_with_US/pinout.h @@ -0,0 +1,30 @@ +#ifndef PINOUT_H +#define PINOUT_H + +// Pins defined by the UCMotor header +#include "UCMotor.h" + +//Pin definitions for the ultrasonic sensor(MB1202) +#define SCL_PIN 5 //Default SDA is Pin5 PORTC for the UNO -- you can set this to any tristate pin +#define SCL_PORT PORTC +#define SDA_PIN 4 //Default SCL is Pin4 PORTC for the UNO -- you can set this to any tristate pin +#define SDA_PORT PORTC + +// Pin definitions for the RC receiver +#ifdef __AVR_ATmega328P__ + #define RC_RIGHT_CHANNEL_PIN 5 + #define RC_LEFT_CHANNEL_PIN 6 + #define RC_SWA_CHANNEL_PIN 9 + #define RC_SWB_CHANNEL_PIN 10 + +#elif defined(__AVR_ATmega2560__) + #define RC_RIGHT_CHANNEL_PIN 44 + #define RC_LEFT_CHANNEL_PIN 45 + #define RC_SWA_CHANNEL_PIN 46 + #define RC_SWB_CHANNEL_PIN 47 + +#else + #error "No RC pin configurations available" +#endif + +#endif // !PINOUT_H diff --git a/vehicle_interface/RC_with_US/rc_commands.cpp b/vehicle_interface/RC_with_US/rc_commands.cpp new file mode 100644 index 00000000..61af19f2 --- /dev/null +++ b/vehicle_interface/RC_with_US/rc_commands.cpp @@ -0,0 +1,67 @@ +#include "rc_commands.h" +#include "util.h" + + +/** + * @brief fetches RC commands and returns motor commands + * + * @return returns array of motor commands + */ +Array fetch_rc_commands() +{ + // Get commands + uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); + uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); + uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); + uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); + + // define temp variables + float right_duty = 0.0; + bool right_dir = DIR_FW; // default direction + float left_duty = 0.0; + bool left_dir = DIR_FW; // default direction + bool autonomy_mode = false; + + /* Switches */ + // Check if stop asserted + if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { + return all_brake_command; // fix to also return mode + } + // Check if mode changed + if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { + autonomy_mode = true; + } + + /* Right side longitudinal wheel set */ + // Forward + if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { + // map the duty from 0 to 1 given the min and max threshold values + right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) + { + right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); + right_dir = DIR_BW; + } + /* Left side longitudinal wheel set */ + // Forward + if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { + left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) + { + left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); + left_dir = DIR_BW; + } + + /* Define and fill wheel motor commands */ + Array wheel_motor_commands = {{ + {left_duty, RELEASE_BRAKE, left_dir}, + {right_duty, RELEASE_BRAKE, right_dir}, + }}; + + /* Return reference to the command set */ + return wheel_motor_commands; +} diff --git a/vehicle_interface/RC_with_US/rc_commands.h b/vehicle_interface/RC_with_US/rc_commands.h new file mode 100644 index 00000000..c47125c0 --- /dev/null +++ b/vehicle_interface/RC_with_US/rc_commands.h @@ -0,0 +1,28 @@ +#ifndef RC_COMMANDS_H +#define RC_COMMANDS_H +#include "pinout.h" +#include "wheel.h" +#include "util.h" + +/* Define the max and min thresholds for the longitudinal wheel sets */ +// CH2 - right longitudinal wheel set +#define RC_RIGHT_SET_FW_MAX 1985 +#define RC_RIGHT_SET_FW_MIN 1638 +#define RC_RIGHT_SET_BW_MAX 1310 +#define RC_RIGHT_SET_BW_MIN 980 +// CH3 - left longitudinal wheel set +#define RC_LEFT_SET_FW_MAX 1972 +#define RC_LEFT_SET_FW_MIN 1632 +#define RC_LEFT_SET_BW_MAX 1320 +#define RC_LEFT_SET_BW_MIN 997//1007 +/* Define channels and thresholds for the switches */ +// CH5/6 - SWA/B +#define RC_SWX_LOW_MAX 1000//994 +#define RC_SWX_LOW_MIN 950//987 +#define RC_SWX_HIGH_MAX 2000//1981 +#define RC_SWX_HIGH_MIN 1900//1974 + +/* Function prototypes for rc commands */ +Array fetch_rc_commands(void); + +#endif // !RC_COMMANDS_H \ No newline at end of file diff --git a/vehicle_interface/RC_with_US/util.cpp b/vehicle_interface/RC_with_US/util.cpp new file mode 100644 index 00000000..17dc0b7a --- /dev/null +++ b/vehicle_interface/RC_with_US/util.cpp @@ -0,0 +1,14 @@ +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} diff --git a/vehicle_interface/RC_with_US/util.h b/vehicle_interface/RC_with_US/util.h new file mode 100644 index 00000000..40a279e9 --- /dev/null +++ b/vehicle_interface/RC_with_US/util.h @@ -0,0 +1,53 @@ +#ifndef UTIL_H +#define UTIL_H + +#include + +#define LN_SCALE_FACTOR -3.0 +#define MAX_DUTY 0.95 + +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); + +template +struct Array { + // Storage + T data[N]; + + static size_t length() { return N; } + using type = T; + + // Item access + T &operator[](size_t index) { return data[index]; } + const T &operator[](size_t index) const { return data[index]; } + + // Iterators + T *begin() { return &data[0]; } + const T *begin() const { return &data[0]; } + T *end() { return &data[N]; } + const T *end() const { return &data[N]; } + + // Comparisons + bool operator==(const Array &rhs) const { + if (this == &rhs) + return true; + for (size_t i = 0; i < N; i++) + if ((*this)[i] != rhs[i]) + return false; + return true; + } + bool operator!=(const Array &rhs) const { + return !(*this == rhs); + } +}; + +#endif // UTIL_H diff --git a/vehicle_interface/RC_with_US/wheel.h b/vehicle_interface/RC_with_US/wheel.h new file mode 100644 index 00000000..3a08dd4a --- /dev/null +++ b/vehicle_interface/RC_with_US/wheel.h @@ -0,0 +1,51 @@ +#ifndef Wheel_h +#define Wheel_h + +#include +#include "pinout.h" +#include "util.h" +#include "UCMotor.h" + +/* Define Important Values */ +#if defined(__AVR_ATmega328P__) + #define TOP 0XFF + #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)) + #define TOP 0x03FF // Top value of the registers (configuration done in MAIN) +#endif + + +/** + * @brief motor command structure that includes duty, brake, and direction + * + */ +typedef struct wheel_motor_command_t +{ + float duty_cycle; // float from 0 to 1 + bool brake_release; + bool dir; +}; + +// Direction values +#define DIR_FW true +#define DIR_BW false +#define ENGAGE_BRAKE false +#define RELEASE_BRAKE true + + +/* Else, define values for mini rc */ +#define NUM_WHEELS 2 // driving with only two drive signals + +typedef enum wheel_index_t +{ + Start = -1, + L = 0U, + R = 1U, + End = 2U, +} wheel_indices; + +const Array all_brake_command = {{ + {0.0, ENGAGE_BRAKE, FORWARD}, + {0.0, ENGAGE_BRAKE, FORWARD} +}}; + +#endif // !Wheel_h From 4f89019ee6a52f1282e6470b5e422cd48dede3f1 Mon Sep 17 00:00:00 2001 From: zhaoshengEE Date: Sun, 22 Mar 2020 19:58:08 -0700 Subject: [PATCH 2/6] Remove the I2C Library and add a readme file --- vehicle_interface/RC_with_US/README.md | 8 + vehicle_interface/RC_with_US/SoftI2CMaster.h | 862 ------------------- 2 files changed, 8 insertions(+), 862 deletions(-) create mode 100644 vehicle_interface/RC_with_US/README.md delete mode 100644 vehicle_interface/RC_with_US/SoftI2CMaster.h diff --git a/vehicle_interface/RC_with_US/README.md b/vehicle_interface/RC_with_US/README.md new file mode 100644 index 00000000..5ce7f88f --- /dev/null +++ b/vehicle_interface/RC_with_US/README.md @@ -0,0 +1,8 @@ +# PropBot +An I2C library is necessary for this project. + +Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster + +And install the library onto Arduino IDE. + +For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries \ No newline at end of file diff --git a/vehicle_interface/RC_with_US/SoftI2CMaster.h b/vehicle_interface/RC_with_US/SoftI2CMaster.h deleted file mode 100644 index 68de1a9a..00000000 --- a/vehicle_interface/RC_with_US/SoftI2CMaster.h +++ /dev/null @@ -1,862 +0,0 @@ -/* Arduino SoftI2C library. - * - * Version 1.4 - * - * Copyright (C) 2013, Bernhard Nebel and Peter Fleury - * - * This is a very fast and very light-weight software I2C-master library - * written in assembler. It is based on Peter Fleury's I2C software - * library: http://homepage.hispeed.ch/peterfleury/avr-software.html - * Recently, the hardware implementation has been added to the code, - * which can be enabled by defining I2C_HARDWARE. - * - * This Library is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This Library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with the Arduino I2cMaster Library. If not, see - * . - */ - -/* In order to use the library, you need to define SDA_PIN, SCL_PIN, - * SDA_PORT and SCL_PORT before including this file. Have a look at - * http://www.arduino.cc/en/Reference/PortManipulation for finding out - * which values to use. For example, if you use digital pin 3 (corresponding - * to PD3) for SDA and digital pin 13 (corresponding to PB5) - * for SCL on a standard Arduino, - * you have to use the following definitions: - * #define SDA_PIN 3 - * #define SDA_PORT PORTD - * #define SCL_PIN 5 - * #define SCL_PORT PORTB - * - * Alternatively, you can define the compile time constant I2C_HARDWARE, - * in which case the TWI hardware is used. In this case you have to use - * the standard SDA/SCL pins (and, of course, the chip needs to support - * this). - * - * You can also define the following constants (see also below): - ' - I2C_PULLUP = 1 meaning that internal pullups should be used - * - I2C_CPUFREQ, when changing CPU clock frequency dynamically - * - I2C_FASTMODE = 1 meaning that the I2C bus allows speeds up to 400 kHz - * - I2C_SLOWMODE = 1 meaning that the I2C bus will allow only up to 25 kHz - * - I2C_NOINTERRUPT = 1 in order to prohibit interrupts while - * communicating (see below). This can be useful if you use the library - * for communicationg with SMbus devices, which have timeouts. - * Note, however, that interrupts are disabled from issuing a start condition - * until issuing a stop condition. So use this option with care! - * - I2C_TIMEOUT = 0..10000 msec in order to return from the I2C functions - * in case of a I2C bus lockup (i.e., SCL constantly low). 0 means no timeout. - * - I2C_MAXWAIT = 0..32767 number of retries in i2c_start_wait. 0 means never stop. - */ - -/* Changelog: - * Version 2.1 - * - added conditional to check whether it is the right MCU type - * Version 2.0 - * - added hardware support as well. - * Version 1.4: - * - added "maximum retry" in i2c_start_wait in order to avoid lockup - * - added "internal pullups", but be careful since the option stretches the I2C specs - * Version 1.3: - * - added "__attribute__ ((used))" for all functions declared with "__attribute__ ((noinline))" - * Now the module is also usable in Arduino 1.6.11+ - * Version 1.2: - * - added pragma to avoid "unused parameter warnings" (suggestion by Walter) - * - replaced wrong license file - * Version 1.1: - * - removed I2C_CLOCK_STRETCHING - * - added I2C_TIMEOUT time in msec (0..10000) until timeout or 0 if no timeout - * - changed i2c_init to return true iff both SDA and SCL are high - * - changed interrupt disabling so that the previous IRQ state is restored - * Version 1.0: basic functionality - */ - -#ifndef __AVR_ARCH__ -#error "Not an AVR MCU! Use 'SlowSoftI2CMaster' library instead of 'SoftI2CMaster'!" -#else - -#ifndef _SOFTI2C_H -#define _SOFTI2C_H 1 - -#include -#include -#include - -#pragma GCC diagnostic push - -#pragma GCC diagnostic ignored "-Wunused-parameter" - - - -// Init function. Needs to be called once in the beginning. -// Returns false if SDA or SCL are low, which probably means -// a I2C bus lockup or that the lines are not pulled up. -bool __attribute__ ((noinline)) i2c_init(void) __attribute__ ((used)); - -// Start transfer function: is the 8-bit I2C address (including the R/W -// bit). -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_start(uint8_t addr) __attribute__ ((used)); - -// Similar to start function, but wait for an ACK! Will timeout if I2C_MAXWAIT > 0. -bool __attribute__ ((noinline)) i2c_start_wait(uint8_t addr) __attribute__ ((used)); - -// Repeated start function: After having claimed the bus with a start condition, -// you can address another or the same chip again without an intervening -// stop condition. -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_rep_start(uint8_t addr) __attribute__ ((used)); - -// Issue a stop condition, freeing the bus. -void __attribute__ ((noinline)) i2c_stop(void) asm("ass_i2c_stop") __attribute__ ((used)); - -// Write one byte to the slave chip that had been addressed -// by the previous start call. is the byte to be sent. -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_write(uint8_t value) asm("ass_i2c_write") __attribute__ ((used)); - - -// Read one byte. If is true, we send a NAK after having received -// the byte in order to terminate the read sequence. -uint8_t __attribute__ ((noinline)) i2c_read(bool last) __attribute__ ((used)); - -// If you want to use the TWI hardeware, you have to define I2C_HARDWARE to be 1 -#ifndef I2C_HARDWARE -#define I2C_HARDWARE 0 -#endif - -#if I2C_HARDWARE -#ifndef TWDR -#error This chip does not support hardware I2C. Please undfine I2C_HARDWARE -#endif -#endif - -// You can set I2C_CPUFREQ independently of F_CPU if you -// change the CPU frequency on the fly. If you do not define it, -// it will use the value of F_CPU -#ifndef I2C_CPUFREQ -#define I2C_CPUFREQ F_CPU -#endif - -// If I2C_FASTMODE is set to 1, then the highest possible frequency below 400kHz -// is selected. Be aware that not all slave chips may be able to deal with that! -#ifndef I2C_FASTMODE -#define I2C_FASTMODE 0 -#endif - -// If I2C_FASTMODE is not defined or defined to be 0, then you can set -// I2C_SLOWMODE to 1. In this case, the I2C frequency will not be higher -// than 25KHz. This could be useful for problematic buses with high pull-ups -// and high capasitance. -#ifndef I2C_SLOWMODE -#define I2C_SLOWMODE 0 -#endif - -// If I2C_PULLUP is set to 1, then the internal pull-up resistors are used. -// This does not conform with the I2C specs, since the bus lines will be -// temporarily in high-state and the internal resistors have roughly 50k. -// With low bus speeds und short buses it usually works, though (hopefully). -#ifndef I2C_PULLUP -#define I2C_PULLUP 0 -#endif - -// if I2C_NOINTERRUPT is 1, then the I2C routines are not interruptable. -// This is most probably only necessary if you are using a 1MHz system clock, -// you are communicating with a SMBus device, and you want to avoid timeouts. -// Be aware that the interrupt bit is enabled after each call. So the -// I2C functions should not be called in interrupt routines or critical regions. -#ifndef I2C_NOINTERRUPT -#define I2C_NOINTERRUPT 0 -#endif - -// I2C_TIMEOUT can be set to a value between 1 and 10000. -// If it is defined and nonzero, it leads to a timeout if the -// SCL is low longer than I2C_TIMEOUT milliseconds, i.e., max timeout is 10 sec -#ifndef I2C_TIMEOUT -#define I2C_TIMEOUT 0 -#else -#if I2C_TIMEOUT > 10000 -#error I2C_TIMEOUT is too large -#endif -#endif - -// I2C_MAXWAIT can be set to any value between 0 and 32767. 0 means no time out. -#ifndef I2C_MAXWAIT -#define I2C_MAXWAIT 500 -#else -#if I2C_MAXWAIT > 32767 || I2C_MAXWAIT < 0 -#error Illegal I2C_MAXWAIT value -#endif -#endif - -#define I2C_TIMEOUT_DELAY_LOOPS (I2C_CPUFREQ/1000UL)*I2C_TIMEOUT/4000UL -#if I2C_TIMEOUT_DELAY_LOOPS < 1 -#define I2C_MAX_STRETCH 1 -#else -#if I2C_TIMEOUT_DELAY_LOOPS > 60000UL -#define I2C_MAX_STRETCH 60000UL -#else -#define I2C_MAX_STRETCH I2C_TIMEOUT_DELAY_LOOPS -#endif -#endif - -#if I2C_FASTMODE -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/350000L)/2-18)/3) -#define SCL_CLOCK 400000UL -#else -#if I2C_SLOWMODE -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/23500L)/2-18)/3) -#define SCL_CLOCK 25000UL -#else -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/90000L)/2-18)/3) -#define SCL_CLOCK 100000UL -#endif -#endif - -// constants for reading & writing -#define I2C_READ 1 -#define I2C_WRITE 0 - -#if !I2C_HARDWARE -// map the IO register back into the IO address space -#define SDA_DDR (_SFR_IO_ADDR(SDA_PORT) - 1) -#define SCL_DDR (_SFR_IO_ADDR(SCL_PORT) - 1) -#define SDA_OUT _SFR_IO_ADDR(SDA_PORT) -#define SCL_OUT _SFR_IO_ADDR(SCL_PORT) -#define SDA_IN (_SFR_IO_ADDR(SDA_PORT) - 2) -#define SCL_IN (_SFR_IO_ADDR(SCL_PORT) - 2) - -#ifndef __tmp_reg__ -#define __tmp_reg__ 0 -#endif - -// Internal delay functions. -void __attribute__ ((noinline)) i2c_delay_half(void) asm("ass_i2c_delay_half") __attribute__ ((used)); -void __attribute__ ((noinline)) i2c_wait_scl_high(void) asm("ass_i2c_wait_scl_high") __attribute__ ((used)); - -void i2c_delay_half(void) -{ // function call 3 cycles => 3C -#if I2C_DELAY_COUNTER < 1 - __asm__ __volatile__ (" ret"); - // 7 cycles for call and return -#else - __asm__ __volatile__ - ( - " ldi r25, %[DELAY] ;load delay constant ;; 4C \n\t" - "_Lidelay: \n\t" - " dec r25 ;decrement counter ;; 4C+xC \n\t" - " brne _Lidelay ;;5C+(x-1)2C+xC\n\t" - " ret ;; 9C+(x-1)2C+xC = 7C+xC" - : : [DELAY] "M" I2C_DELAY_COUNTER : "r25"); - // 7 cycles + 3 times x cycles -#endif -} - -void i2c_wait_scl_high(void) -{ -#if I2C_TIMEOUT <= 0 - __asm__ __volatile__ - ("_Li2c_wait_stretch: \n\t" - " sbis %[SCLIN],%[SCLPIN] ;wait for SCL high \n\t" - " rjmp _Li2c_wait_stretch \n\t" - " cln ;signal: no timeout \n\t" - " ret " - : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN)); -#else - __asm__ __volatile__ - ( " ldi r27, %[HISTRETCH] ;load delay counter \n\t" - " ldi r26, %[LOSTRETCH] \n\t" - "_Lwait_stretch: \n\t" - " clr __tmp_reg__ ;do next loop 255 times \n\t" - "_Lwait_stretch_inner_loop: \n\t" - " rcall _Lcheck_scl_level ;call check function ;; 12C \n\t" - " brpl _Lstretch_done ;done if N=0 ;; +1 = 13C\n\t" - " dec __tmp_reg__ ;dec inner loop counter;; +1 = 14C\n\t" - " brne _Lwait_stretch_inner_loop ;; +2 = 16C\n\t" - " sbiw r26,1 ;dec outer loop counter \n\t" - " brne _Lwait_stretch ;continue with outer loop \n\t" - " sen ;timeout -> set N-bit=1 \n\t" - " rjmp _Lwait_return ;and return with N=1\n\t" - "_Lstretch_done: ;SCL=1 sensed \n\t" - " cln ;OK -> clear N-bit \n\t" - " rjmp _Lwait_return ; and return with N=0 \n\t" - - "_Lcheck_scl_level: ;; call = 3C\n\t" - " cln ;; +1C = 4C \n\t" - " sbic %[SCLIN],%[SCLPIN] ;skip if SCL still low ;; +2C = 6C \n\t" - " rjmp _Lscl_high ;; +0C = 6C \n\t" - " sen ;; +1 = 7C\n\t " - "_Lscl_high: " - " nop ;; +1C = 8C \n\t" - " ret ;return N-Bit=1 if low ;; +4 = 12C\n\t" - - "_Lwait_return:" - : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), - [HISTRETCH] "M" (I2C_MAX_STRETCH>>8), - [LOSTRETCH] "M" (I2C_MAX_STRETCH&0xFF) - : "r26", "r27"); -#endif -} -#endif // !I2C_HARDWARE - -bool i2c_init(void) -#if I2C_HARDWARE -{ -#if I2C_PULLUP - digitalWrite(SDA, 1); - digitalWrite(SCL, 1); -#else - digitalWrite(SDA, 0); - digitalWrite(SCL, 0); -#endif -#if ((I2C_CPUFREQ/SCL_CLOCK)-16)/2 < 250 - TWSR = 0; /* no prescaler */ - TWBR = ((I2C_CPUFREQ/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ -#else - TWSR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_START) && (twst != TW_REP_START)) return false; - - // send device address - TWDR = addr; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return false; - - return true; -} -#else -{ - __asm__ __volatile__ - ( -#if I2C_NOINTERRUPT - " cli ;clear IRQ bit \n\t" -#endif - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;wait T/2 \n\t" - " rcall ass_i2c_write ;now write address \n\t" - " ret" - : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SCLIN] "I" (SCL_IN),[SCLPIN] "I" (SCL_PIN)); - return true; // we never return here! -} -#endif - -bool i2c_rep_start(uint8_t addr) -#if I2C_HARDWARE -{ - return i2c_start(addr); -} -#else -{ - __asm__ __volatile__ - - ( -#if I2C_NOINTERRUPT - " cli \n\t" -#endif -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable SDA pull-up\n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " rcall ass_i2c_write \n\t" - " ret" - : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), - [SCLIN] "I" (SCL_IN), [SCLOUT] "I" (SCL_OUT), [SDAOUT] "I" (SDA_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); - return true; // just to fool the compiler -} -#endif - -bool i2c_start_wait(uint8_t addr) -#if I2C_HARDWARE -{ - uint8_t twst; - uint16_t maxwait = I2C_MAXWAIT; -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - while (true) { - // send START condition - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_START) && (twst != TW_REP_START)) continue; - - // send device address - TWDR = addr; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) - { - /* device busy, send stop condition to terminate write operation */ - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - if (maxwait) - if (--maxwait == 0) - return false; - - continue; - } - //if( twst != TW_MT_SLA_ACK) return 1; - return true; - } -} -#else -{ - __asm__ __volatile__ - ( - " push r24 ;save original parameter \n\t" -#if I2C_MAXWAIT - " ldi r31, %[HIMAXWAIT] ;load max wait counter \n\t" - " ldi r30, %[LOMAXWAIT] ;load low byte \n\t" -#endif - "_Li2c_start_wait1: \n\t" - " pop r24 ;restore original parameter\n\t" - " push r24 ;and save again \n\t" -#if I2C_NOINTERRUPT - " cli ;disable interrupts \n\t" -#endif - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " rcall ass_i2c_write ;write address \n\t" - " tst r24 ;if device not busy -> done \n\t" - " brne _Li2c_start_wait_done \n\t" - " rcall ass_i2c_stop ;terminate write & enable IRQ \n\t" -#if I2C_MAXWAIT - " sbiw r30,1 ;decrement max wait counter\n\t" - " breq _Li2c_start_wait_done ;if zero reached, exit with false -> r24 already zero!\n\t" -#endif - " rjmp _Li2c_start_wait1 ;device busy, poll ack again \n\t" - "_Li2c_start_wait_done: \n\t" - " clr r25 ;clear high byte of return value\n\t" - " pop __tmp_reg__ ;pop off orig argument \n\t" - " ret " - : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAOUT] "I" (SDA_OUT), - [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), - [HIMAXWAIT] "M" (I2C_MAXWAIT>>8), - [LOMAXWAIT] "M" (I2C_MAXWAIT&0xFF) - : "r30", "r31" ); -} -#endif - -void i2c_stop(void) -#if I2C_HARDWARE -{ -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - /* send stop condition */ - TWCR = (1< I2C_TIMEOUT) return; -#endif - } -} -#else -{ - __asm__ __volatile__ - ( -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;T/2 delay \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;T/2 delay \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rcall ass_i2c_delay_half \n\t" -#if I2C_NOINTERRUPT - " sei ;enable interrupts again!\n\t" -#endif - : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); -} -#endif - - -bool i2c_write(uint8_t value) -#if I2C_HARDWARE -{ - uint8_t twst; -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - - // send data to the previously addressed device - TWDR = value; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits - twst = TW_STATUS & 0xF8; - if( twst != TW_MT_DATA_ACK) return false; - return true; -} -#else -{ - __asm__ __volatile__ - ( - " sec ;set carry flag \n\t" - " rol r24 ;shift in carry and shift out MSB \n\t" - " rjmp _Li2c_write_first \n\t" - "_Li2c_write_bit:\n\t" - " lsl r24 ;left shift into carry ;; 1C\n\t" - "_Li2c_write_first:\n\t" - " breq _Li2c_get_ack ;jump if TXreg is empty;; +1 = 2C \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 4C \n\t" - " nop \n\t" - " nop \n\t" - " nop \n\t" - " brcc _Li2c_write_low ;;+1/+2=5/6C\n\t" - " nop ;; +1 = 7C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA ;; +2 = 9C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rjmp _Li2c_write_high ;; +2 = 11C \n\t" - "_Li2c_write_low: \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; +2 = 9C \n\t" - " rjmp _Li2c_write_high ;;+2 = 11C \n\t" - "_Li2c_write_high: \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;;+X = 11C+X\n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;;+2 = 13C+X\n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " cln ;clear N-bit ;;+1 = 14C+X\n\t" - " nop \n\t" - " nop \n\t" - " nop \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for SCL high ;;+2 = 16C+X\n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brpl _Ldelay_scl_high ;;+2 = 18C+X\n\t" - "_Li2c_write_return_false: \n\t" - " clr r24 ; return false because of timeout \n\t" - " rjmp _Li2c_write_return \n\t" - "_Ldelay_scl_high: \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;;+X= 18C+2X\n\t" -#endif - " rjmp _Li2c_write_bit \n\t" - " ;; +2 = 20C +2X for one bit-loop \n\t" - "_Li2c_get_ack: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 5C \n\t" - " nop \n\t" - " nop \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA ;;+2 = 7C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; +X = 7C+X \n\t" -#endif - " clr r25 ;; 17C+2X \n\t" - " clr r24 ;return 0 ;; 14C + X \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; +2 = 9C+X\n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - "_Li2c_ack_wait: \n\t" - " cln ; clear N-bit ;; 10C + X\n\t" - " nop \n\t" - " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X \n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brmi _Li2c_write_return_false ;; 13C + X \n\t " - " sbis %[SDAIN],%[SDAPIN] ;if SDA hi -> return 0 ;; 15C + X \n\t" - " ldi r24,1 ;return true ;; 16C + X \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 16C + 2X \n\t" -#endif - "_Li2c_write_return: \n\t" - " nop \n\t " - " nop \n\t " -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" - " ret \n\t" - " ;; + 4 = 17C + 2X for acknowldge bit" - :: - [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN)); - return true; // fooling the compiler -} -#endif - -uint8_t i2c_read(bool last) -#if I2C_HARDWARE -{ -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - TWCR = (1< I2C_TIMEOUT) return 0xFF; -#endif - } - return TWDR; -} -#else -{ - __asm__ __volatile__ - ( - " ldi r23,0x01 \n\t" - "_Li2c_read_bit: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA(prev. ACK);; 4C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " nop \n\t" - " nop \n\t" - " nop \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 4C+X \n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 6C + X \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 6C + 2X \n\t" -#endif - " cln ; clear N-bit ;; 7C + 2X \n\t" - " nop \n\t " - " nop \n\t " - " nop \n\t " - " sbis %[SCLIN], %[SCLPIN] ;check for SCL high ;; 9C +2X \n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brmi _Li2c_read_return ;return if timeout ;; 10C + 2X\n\t" - " clc ;clear carry flag ;; 11C + 2X\n\t" - " sbic %[SDAIN],%[SDAPIN] ;if SDA is high ;; 11C + 2X\n\t" - " sec ;set carry flag ;; 12C + 2X\n\t" - " rol r23 ;store bit ;; 13C + 2X\n\t" - " brcc _Li2c_read_bit ;while receiv reg not full \n\t" - " ;; 15C + 2X for one bit loop \n\t" - - "_Li2c_put_ack: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" - " cpi r24,0 ;; 3C \n\t" - " breq _Li2c_put_ack_low ;if (ack=0) ;; 5C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rjmp _Li2c_put_ack_high \n\t" - "_Li2c_put_ack_low: ;else \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; 7C \n\t" - "_Li2c_put_ack_high: \n\t" - " nop \n\t " - " nop \n\t " - " nop \n\t " -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 7C + X \n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 9C +X \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " cln ;clear N ;; +1 = 10C\n\t" - " nop \n\t " - " nop \n\t " - " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X\n\t" - " rcall ass_i2c_wait_scl_high \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 11C + 2X\n\t" -#endif - "_Li2c_read_return: \n\t" - " nop \n\t " - " nop \n\t " -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - "sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" - " mov r24,r23 ;; 12C + 2X \n\t" - " clr r25 ;; 13 C + 2X\n\t" - " ret ;; 17C + X" - :: - [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN) - ); - return ' '; // fool the compiler! -} -#endif - -#pragma GCC diagnostic pop - -#endif -#endif From 519eee9bc3ca6b137e37c689bd11b0e91fddbf82 Mon Sep 17 00:00:00 2001 From: zhaoshengEE Date: Sun, 22 Mar 2020 21:48:14 -0700 Subject: [PATCH 3/6] Removed UCMotor files --- vehicle_interface/RC_with_US/README.md | 4 +- vehicle_interface/RC_with_US/UCMotor.cpp | 729 ----------------------- vehicle_interface/RC_with_US/UCMotor.h | 195 ------ 3 files changed, 3 insertions(+), 925 deletions(-) delete mode 100644 vehicle_interface/RC_with_US/UCMotor.cpp delete mode 100644 vehicle_interface/RC_with_US/UCMotor.h diff --git a/vehicle_interface/RC_with_US/README.md b/vehicle_interface/RC_with_US/README.md index 5ce7f88f..40a6e4f3 100644 --- a/vehicle_interface/RC_with_US/README.md +++ b/vehicle_interface/RC_with_US/README.md @@ -5,4 +5,6 @@ Please download the SoftI2CMaster.h from the following github link: https://gith And install the library onto Arduino IDE. -For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries \ No newline at end of file +For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries + +Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car \ No newline at end of file diff --git a/vehicle_interface/RC_with_US/UCMotor.cpp b/vehicle_interface/RC_with_US/UCMotor.cpp deleted file mode 100644 index d3e6501a..00000000 --- a/vehicle_interface/RC_with_US/UCMotor.cpp +++ /dev/null @@ -1,729 +0,0 @@ -/** - * @file UCMotor.cpp - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ - -// UCTRONOICS Motor shield library -// this code is public domain, enjoy! - - -#if (ARDUINO >= 100) - #include "Arduino.h" -#else - #if defined(__AVR__) - #include - #endif - #include "WProgram.h" -#endif - -#include "UCMotor.h" - - - -static uint8_t latch_state; - -#if (MICROSTEPS == 8) -uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; -#elif (MICROSTEPS == 16) -uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; -#endif - -UCMotorController::UCMotorController(void) { - TimerInitalized = false; -} - -void UCMotorController::enable(void) { - // setup the latch - /* - LATCH_DDR |= _BV(LATCH); - ENABLE_DDR |= _BV(ENABLE); - CLK_DDR |= _BV(CLK); - SER_DDR |= _BV(SER); - */ - pinMode(MOTORLATCH, OUTPUT); - pinMode(MOTORENABLE, OUTPUT); - pinMode(MOTORDATA, OUTPUT); - pinMode(MOTORCLK, OUTPUT); - - latch_state = 0; - - latch_tx(); // "reset" - - //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! - digitalWrite(MOTORENABLE, LOW); -} - - -void UCMotorController::latch_tx(void) { - uint8_t i; - - //LATCH_PORT &= ~_BV(LATCH); - digitalWrite(MOTORLATCH, LOW); - - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - - for (i=0; i<8; i++) { - //CLK_PORT &= ~_BV(CLK); - digitalWrite(MOTORCLK, LOW); - - if (latch_state & _BV(7-i)) { - //SER_PORT |= _BV(SER); - digitalWrite(MOTORDATA, HIGH); - } else { - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - } - //CLK_PORT |= _BV(CLK); - digitalWrite(MOTORCLK, HIGH); - } - //LATCH_PORT |= _BV(LATCH); - digitalWrite(MOTORLATCH, HIGH); -} - -static UCMotorController MC; - -/****************************************** - MOTORS -******************************************/ -inline void initPWM1(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a - TCCR2B = freq & 0x7; - OCR2A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a - TCCR1B = (freq & 0x7) | _BV(WGM12); - OCR1A = 0; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(9, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase - OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC4RS = 0x0000; - OC4R = 0x0000; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(10, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase - OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC5RS = 0x0000; - OC5R = 0x0000; - #else - // If we are not using PWM for pin 11, then just do digital - digitalWrite(11, LOW); - #endif -#else - #error "This chip is not supported!" -#endif - #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) - pinMode(11, OUTPUT); - //Serial .println("OK"); - #endif -} - -inline void setPWM1(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR1A = s; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 - OC4RS = s; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 - OC5RS = s; - #else - // If we are not doing PWM output for M1, then just use on/off - if (s > 127) - { - digitalWrite(11, HIGH); - } - else - { - digitalWrite(11, LOW); - } - #endif -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM2(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2B (pin 3) - TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b - TCCR2B = freq & 0x7; - OCR2B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 3 is now PE5 (OC3C) - TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c - TCCR3B = (freq & 0x7) | _BV(WGM12); - OCR3C = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase - OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC1RS = 0x0000; - OC1R = 0x0000; -#else - #error "This chip is not supported!" -#endif - - pinMode(3, OUTPUT); -} - -inline void setPWM2(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR3C = s; -#elif defined(__PIC32MX__) - // Set the OC1 (pin3) PMW duty cycle from 0 to 255 - OC1RS = s; -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM3(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A / PD6 (pin 6) - TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A - //TCCR0B = freq & 0x7; - OCR0A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a - TCCR4B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR4A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase - OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC3RS = 0x0000; - OC3R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(6, OUTPUT); -} - -inline void setPWM3(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR4A = s; -#elif defined(__PIC32MX__) - // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 - OC3RS = s; -#else - #error "This chip is not supported!" -#endif -} - - - -inline void initPWM4(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0B / PD5 (pin 5) - TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a - //TCCR0B = freq & 0x7; - OCR0B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 5 is now PE3 (OC3A) - TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a - TCCR3B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR3A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase - OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC2RS = 0x0000; - OC2R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(5, OUTPUT); -} - -inline void setPWM4(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR3A = s; -#elif defined(__PIC32MX__) - // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 - OC2RS = s; -#else - #error "This chip is not supported!" -#endif -} - -UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { - motornum = num; - pwmfreq = freq; - MC.enable(); - - switch (num) { - case 1: - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM1(freq); - break; - case 2: - latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM2(freq); - break; - case 3: - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM3(freq); - break; - case 4: - latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM4(freq); - break; - } -} - -void UC_DCMotor::run(uint8_t cmd) { - uint8_t a, b; - switch (motornum) { - case 1: - a = MOTOR1_A; b = MOTOR1_B; break; - case 2: - a = MOTOR2_A; b = MOTOR2_B; break; - case 3: - a = MOTOR3_A; b = MOTOR3_B; break; - case 4: - a = MOTOR4_A; b = MOTOR4_B; break; - default: - return; - } - - switch (cmd) { - case FORWARD: - if(motornum ==3 || motornum ==4){ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - }else{ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - } - MC.latch_tx(); - break; - case BACKWARD: - if(motornum ==3 || motornum ==4){ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - }else{ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - } - MC.latch_tx(); - break; - case LEFT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx(); - } - break; - case RIGHT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - } - break; - case STOP: - latch_state &= ~_BV(a); // A and B both low - latch_state &= ~_BV(b); - MC.latch_tx(); - break; - } -} - -void UC_DCMotor::setSpeed(uint8_t speed) { - switch (motornum) { - case 1: - setPWM1(speed); break; - case 2: - setPWM2(speed); break; - case 3: - setPWM3(speed); break; - case 4: - setPWM4(speed); break; - } -} - -/****************************************** - STEPPERS -******************************************/ - -UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { - MC.enable(); - - revsteps = steps; - steppernum = num; - currentstep = 0; - - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(11, OUTPUT); - pinMode(3, OUTPUT); - digitalWrite(11, HIGH); - digitalWrite(3, HIGH); - - // use PWM for microstepping support - initPWM1(STEPPER1_PWM_RATE); - initPWM2(STEPPER1_PWM_RATE); - setPWM1(255); - setPWM2(255); - - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(5, HIGH); - digitalWrite(6, HIGH); - - // use PWM for microstepping support - // use PWM for microstepping support - initPWM3(STEPPER2_PWM_RATE); - initPWM4(STEPPER2_PWM_RATE); - setPWM3(255); - setPWM4(255); - } -} - -void UC_Stepper::setSpeed(uint16_t rpm) { - usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); - steppingcounter = 0; -} - -void UC_Stepper::release(void) { - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - } -} - -void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { - uint32_t uspers = usperstep; - uint8_t ret = 0; - - if (style == INTERLEAVE) { - uspers /= 2; - } - else if (style == MICROSTEP) { - uspers /= MICROSTEPS; - steps *= MICROSTEPS; -#ifdef MOTORDEBUG - Serial.print("steps = "); Serial.println(steps, DEC); -#endif - } - - while (steps--) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - if (style == MICROSTEP) { - while ((ret != 0) && (ret != MICROSTEPS)) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - } -} - -uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { - uint8_t a, b, c, d; - uint8_t ocrb, ocra; - - ocra = ocrb = 255; - - if (steppernum == 1) { - a = _BV(MOTOR1_A); - b = _BV(MOTOR2_A); - c = _BV(MOTOR1_B); - d = _BV(MOTOR2_B); - } else if (steppernum == 2) { - a = _BV(MOTOR3_A); - b = _BV(MOTOR4_A); - c = _BV(MOTOR3_B); - d = _BV(MOTOR4_B); - } else { - return 0; - } - - // next determine what sort of stepping procedure we're up to - if (style == SINGLE) { - if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } - else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next even step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } - else { - currentstep -= MICROSTEPS; - } - } - } else if (style == DOUBLE) { - if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next odd step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } else { - currentstep -= MICROSTEPS; - } - } - } else if (style == INTERLEAVE) { - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } - - if (style == MICROSTEP) { - if (dir == FORWARD) { - currentstep++; - } else { - // BACKWARDS - currentstep--; - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - - ocra = ocrb = 0; - if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { - ocra = microstepcurve[MICROSTEPS - currentstep]; - ocrb = microstepcurve[currentstep]; - } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { - ocra = microstepcurve[currentstep - MICROSTEPS]; - ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; - } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { - ocra = microstepcurve[MICROSTEPS*3 - currentstep]; - ocrb = microstepcurve[currentstep - MICROSTEPS*2]; - } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { - ocra = microstepcurve[currentstep - MICROSTEPS*3]; - ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; - } - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - -#ifdef MOTORDEBUG - Serial.print("current step: "); Serial.println(currentstep, DEC); - Serial.print(" pwmA = "); Serial.print(ocra, DEC); - Serial.print(" pwmB = "); Serial.println(ocrb, DEC); -#endif - - if (steppernum == 1) { - setPWM1(ocra); - setPWM2(ocrb); - } else if (steppernum == 2) { - setPWM3(ocra); - setPWM4(ocrb); - } - - - // release all - latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 - - //Serial.println(step, DEC); - if (style == MICROSTEP) { - if ((currentstep >= 0) && (currentstep < MICROSTEPS)) - latch_state |= a | b; - if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) - latch_state |= b | c; - if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) - latch_state |= c | d; - if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) - latch_state |= d | a; - } else { - switch (currentstep/(MICROSTEPS/2)) { - case 0: - latch_state |= a; // energize coil 1 only - break; - case 1: - latch_state |= a | b; // energize coil 1+2 - break; - case 2: - latch_state |= b; // energize coil 2 only - break; - case 3: - latch_state |= b | c; // energize coil 2+3 - break; - case 4: - latch_state |= c; // energize coil 3 only - break; - case 5: - latch_state |= c | d; // energize coil 3+4 - break; - case 6: - latch_state |= d; // energize coil 4 only - break; - case 7: - latch_state |= d | a; // energize coil 1+4 - break; - } - } - - - MC.latch_tx(); - return currentstep; -} - diff --git a/vehicle_interface/RC_with_US/UCMotor.h b/vehicle_interface/RC_with_US/UCMotor.h deleted file mode 100644 index 1e2f1e05..00000000 --- a/vehicle_interface/RC_with_US/UCMotor.h +++ /dev/null @@ -1,195 +0,0 @@ -/** - * @file UCMotor.h - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ -// UCTRONICS Motor shield library -// this code is public domain, enjoy! - -/* - * Usage Notes: - * For PIC32, all features work properly with the following two exceptions: - * - * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 - * to completely operate (four for motor outputs and two for RC servos), the - * M1 motor output will not have PWM ability when used with a PIC32 board. - * However, there is a very simple workaround. If you need to drive a stepper - * or DC motor with PWM on motor output M1, you can use the PWM output on pin - * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for - * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting - * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the - * two #defines below to activate the PWM on either pin 9 or pin 10. You will - * then have a fully functional microstepping for 2 stepper motors, or four - * DC motor outputs with PWM. - * - * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and - * the operation of DC motors and stepper motors as of 9/2012. This issue - * will get fixed in future MPIDE releases, but at the present time it means - * that the Motor Party example will NOT work properly. Any time you attach - * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will - * stop working. Thus no steppers or DC motors. - * - */ -// 09/15/2012 Modified for use with chipKIT boards - - -#ifndef _UCMotor_h_ -#define _UCMotor_h_ - -#include -#if defined(__AVR__) - #include - - //#define MOTORDEBUG 1 - - #define MICROSTEPS 16 // 8 or 16 - - #define MOTOR12_64KHZ _BV(CS20) // no prescale - #define MOTOR12_8KHZ _BV(CS21) // divide by 8 - #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 - #define MOTOR12_1KHZ _BV(CS22) // divide by 64 - - #define MOTOR34_64KHZ _BV(CS00) // no prescale - #define MOTOR34_8KHZ _BV(CS01) // divide by 8 - #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 - - #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors - #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 - #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 - -#elif defined(__PIC32MX__) - //#define MOTORDEBUG 1 - - // Uncomment the one of following lines if you have put a jumper from - // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. - // Either will enable PWM for M1 - //#define PIC32_USE_PIN9_FOR_M1_PWM - //#define PIC32_USE_PIN10_FOR_M1_PWM - - #define MICROSTEPS 16 // 8 or 16 - - // For PIC32 Timers, define prescale settings by PWM frequency - #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz - - #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz - - // PWM rate for DC motors. - #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ - // Note: for PIC32, both of these must be set to the same value - // since there's only one timebase for all 4 PWM outputs - #define STEPPER1_PWM_RATE MOTOR12_39KHZ - #define STEPPER2_PWM_RATE MOTOR34_39KHZ - -#endif - -// Bit positions in the 74HCT595 shift register output -#define MOTOR1_A 2 -#define MOTOR1_B 3 -#define MOTOR2_A 1 -#define MOTOR2_B 4 -#define MOTOR4_A 0 -#define MOTOR4_B 6 -#define MOTOR3_A 5 -#define MOTOR3_B 7 - -// Constants that the user passes in to the motor calls -#define FORWARD 1 -#define BACKWARD 2 -#define LEFT 3 -#define RIGHT 4 -#define STOP 5 - -#define FORWARD2 2 -#define BACKWARD2 1 - -#define BRAKE 3 -#define RELEASE 4 - -// Constants that the user passes in to the stepper calls -#define SINGLE 1 -#define DOUBLE 2 -#define INTERLEAVE 3 -#define MICROSTEP 4 - -/* -#define LATCH 4 -#define LATCH_DDR DDRB -#define LATCH_PORT PORTB - -#define CLK_PORT PORTD -#define CLK_DDR DDRD -#define CLK 4 - -#define ENABLE_PORT PORTD -#define ENABLE_DDR DDRD -#define ENABLE 7 - -#define SER 0 -#define SER_DDR DDRB -#define SER_PORT PORTB -*/ - -// Arduino pin names for interface to 74HCT595 latch -#define MOTORLATCH 12 -#define MOTORCLK 4 -#define MOTORENABLE 7 -#define MOTORDATA 8 - -class UCMotorController -{ - public: - UCMotorController(void); - void enable(void); - friend class UC_DCMotor; - void latch_tx(void); - uint8_t TimerInitalized; -}; - -class UC_DCMotor -{ - public: - UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); - void run(uint8_t); - void setSpeed(uint8_t); - - private: - uint8_t motornum, pwmfreq; -}; - -class UC_Stepper { - public: - UC_Stepper(uint16_t, uint8_t); - void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); - void setSpeed(uint16_t); - uint8_t onestep(uint8_t dir, uint8_t style); - void release(void); - uint16_t revsteps; // # steps per revolution - uint8_t steppernum; - uint32_t usperstep, steppingcounter; - private: - uint8_t currentstep; - -}; - -uint8_t getlatchstate(void); - -#endif From 19fffa44032fd0ca03f525e46c73d0bc7536fa80 Mon Sep 17 00:00:00 2001 From: zhaoshengEE Date: Fri, 27 Mar 2020 14:35:43 -0700 Subject: [PATCH 4/6] changed directory --- vehicle_interface/RC_with_US/main.ino | 143 -- vehicle_interface/RC_with_US/pinout.h | 30 - vehicle_interface/RC_with_US/rc_commands.cpp | 67 - vehicle_interface/RC_with_US/rc_commands.h | 28 - vehicle_interface/RC_with_US/util.cpp | 14 - vehicle_interface/RC_with_US/util.h | 53 - vehicle_interface/RC_with_US/wheel.h | 51 - .../{RC_with_US => main}/README.md | 3 +- vehicle_interface/main/SoftI2CMaster.h | 862 ++++++++++ vehicle_interface/main/UCMotor.cpp | 1458 ++++++++--------- vehicle_interface/main/UCMotor.h | 390 ++--- vehicle_interface/main/main.ino | 73 + vehicle_interface/main/pinout.h | 7 +- vehicle_interface/main/rc_commands.cpp | 134 +- vehicle_interface/main/rc_commands.h | 54 +- vehicle_interface/main/util.cpp | 28 +- vehicle_interface/main/util.h | 106 +- vehicle_interface/main/wheel.h | 4 +- 18 files changed, 2030 insertions(+), 1475 deletions(-) delete mode 100644 vehicle_interface/RC_with_US/main.ino delete mode 100644 vehicle_interface/RC_with_US/pinout.h delete mode 100644 vehicle_interface/RC_with_US/rc_commands.cpp delete mode 100644 vehicle_interface/RC_with_US/rc_commands.h delete mode 100644 vehicle_interface/RC_with_US/util.cpp delete mode 100644 vehicle_interface/RC_with_US/util.h delete mode 100644 vehicle_interface/RC_with_US/wheel.h rename vehicle_interface/{RC_with_US => main}/README.md (92%) create mode 100644 vehicle_interface/main/SoftI2CMaster.h diff --git a/vehicle_interface/RC_with_US/main.ino b/vehicle_interface/RC_with_US/main.ino deleted file mode 100644 index 1e80305d..00000000 --- a/vehicle_interface/RC_with_US/main.ino +++ /dev/null @@ -1,143 +0,0 @@ -#include "pinout.h" -#include "rc_commands.h" -#include "UCMotor.h" -#include -#include -#include //You will need to install this library - -volatile Array commands = all_brake_command; - -// Initialize the UC motor objects -UC_DCMotor mL(1); -UC_DCMotor mR(2); -UC_DCMotor * minis[2] = { - &mL, - &mR -}; - -void setup() -{ - // For debugging - Serial.begin(9600); - Serial.println("Init"); - - // Setup RC pins in input mode (input capture) - pinMode(RC_RIGHT_CHANNEL_PIN, INPUT); - pinMode(RC_LEFT_CHANNEL_PIN, INPUT); - pinMode(RC_SWA_CHANNEL_PIN, INPUT); - pinMode(RC_SWB_CHANNEL_PIN, INPUT); - - //Initialize I2C protocol for ultrasonic sensor - i2c_init(); - - // Indicate setup done - Serial.println("Setup complete"); -} - -/** - * @brief Sends commands to a UC_DC motor - * - * @param uc_motor reference to the UC_DC motor object - * @param command the command to send - * - * @return - */ -void sendCommandMini(UC_DCMotor * uc_motor, wheel_motor_command_t command) { - // BRAKE condition - if (command.brake_release == ENGAGE_BRAKE) { - uc_motor->run(STOP); - uc_motor->setSpeed(0); - return; - } - - // Step 1: set rotation direction - String debug_str = String(); - if (command.dir == DIR_FW) { - uc_motor->run(FORWARD); - } else { - uc_motor->run(BACKWARD); - } - - // Step 2: set speed - uc_motor->setSpeed(command.duty_cycle * TOP); -} - -void loop() -{ - int distance = read_the_sensor_example(); - if (distance > 25){ - // Update wheel commands - commands = fetch_rc_commands(); - } - - else{ - commands = all_brake_command; - } - // Send commands to wheels - for (int i = wheel_indices::Start + 1; i < wheel_indices::End; i++ ) { - sendCommandMini(minis[i], commands[i]); - } -} - -////////////////////////////////////////////////////////// -// Code Example: Read the sensor at the default address // -////////////////////////////////////////////////////////// -int read_the_sensor_example(){ - boolean error = 0; //Create a bit to check for catch errors as needed. - int distance; - - //Take a range reading at the default address of 224 - error = start_sensor(224); //Start the sensor and collect any error codes. - if (!error){ //If you had an error starting the sensor there is little point in reading it as you will get old data. - delay(100); - distance = read_sensor(224); //reading the sensor will return an integer value -- if this value is 0 there was an error - //Serial.print("Distance:");Serial.println(range);Serial.print("cm"); - Serial.println((String)"Distance:"+distance+" cm"); - return distance; - } -} - -/////////////////////////////////////////////////// -// Function: Start a range reading on the sensor // -/////////////////////////////////////////////////// -//Uses the I2C library to start a sensor at the given address -//Collects and reports an error bit where: 1 = there was an error or 0 = there was no error. -//INPUTS: byte bit8address = the address of the sensor that we want to command a range reading -//OUPUTS: bit errorlevel = reports if the function was successful in taking a range reading: 1 = the function -// had an error, 0 = the function was successful -boolean start_sensor(byte bit8address){ - boolean errorlevel = 0; - bit8address = bit8address & B11111110; //Do a bitwise 'and' operation to force the last bit to be zero -- we are writing to the address. - errorlevel = !i2c_start(bit8address) | errorlevel; //Run i2c_start(address) while doing so, collect any errors where 1 = there was an error. - errorlevel = !i2c_write(81) | errorlevel; //Send the 'take range reading' command. (notice how the library has error = 0 so I had to use "!" (not) to invert the error) - i2c_stop(); - return errorlevel; -} - - - -/////////////////////////////////////////////////////////////////////// -// Function: Read the range from the sensor at the specified address // -/////////////////////////////////////////////////////////////////////// -//Uses the I2C library to read a sensor at the given address -//Collects errors and reports an invalid range of "0" if there was a problem. -//INPUTS: byte bit8address = the address of the sensor to read from -//OUPUTS: int range = the distance in cm that the sensor reported; if "0" there was a communication error -int read_sensor(byte bit8address){ - boolean errorlevel = 0; - int range = 0; - byte range_highbyte = 0; - byte range_lowbyte = 0; - bit8address = bit8address | B00000001; //Do a bitwise 'or' operation to force the last bit to be 'one' -- we are reading from the address. - errorlevel = !i2c_start(bit8address) | errorlevel; - range_highbyte = i2c_read(0); //Read a byte and send an ACK (acknowledge) - range_lowbyte = i2c_read(1); //Read a byte and send a NACK to terminate the transmission - i2c_stop(); - range = (range_highbyte * 256) + range_lowbyte; //compile the range integer from the two bytes received. - if(errorlevel){ - return 0; - } - else{ - return range; - } -} diff --git a/vehicle_interface/RC_with_US/pinout.h b/vehicle_interface/RC_with_US/pinout.h deleted file mode 100644 index 94d3be7a..00000000 --- a/vehicle_interface/RC_with_US/pinout.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef PINOUT_H -#define PINOUT_H - -// Pins defined by the UCMotor header -#include "UCMotor.h" - -//Pin definitions for the ultrasonic sensor(MB1202) -#define SCL_PIN 5 //Default SDA is Pin5 PORTC for the UNO -- you can set this to any tristate pin -#define SCL_PORT PORTC -#define SDA_PIN 4 //Default SCL is Pin4 PORTC for the UNO -- you can set this to any tristate pin -#define SDA_PORT PORTC - -// Pin definitions for the RC receiver -#ifdef __AVR_ATmega328P__ - #define RC_RIGHT_CHANNEL_PIN 5 - #define RC_LEFT_CHANNEL_PIN 6 - #define RC_SWA_CHANNEL_PIN 9 - #define RC_SWB_CHANNEL_PIN 10 - -#elif defined(__AVR_ATmega2560__) - #define RC_RIGHT_CHANNEL_PIN 44 - #define RC_LEFT_CHANNEL_PIN 45 - #define RC_SWA_CHANNEL_PIN 46 - #define RC_SWB_CHANNEL_PIN 47 - -#else - #error "No RC pin configurations available" -#endif - -#endif // !PINOUT_H diff --git a/vehicle_interface/RC_with_US/rc_commands.cpp b/vehicle_interface/RC_with_US/rc_commands.cpp deleted file mode 100644 index 61af19f2..00000000 --- a/vehicle_interface/RC_with_US/rc_commands.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "rc_commands.h" -#include "util.h" - - -/** - * @brief fetches RC commands and returns motor commands - * - * @return returns array of motor commands - */ -Array fetch_rc_commands() -{ - // Get commands - uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); - uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); - uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); - uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); - - // define temp variables - float right_duty = 0.0; - bool right_dir = DIR_FW; // default direction - float left_duty = 0.0; - bool left_dir = DIR_FW; // default direction - bool autonomy_mode = false; - - /* Switches */ - // Check if stop asserted - if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { - return all_brake_command; // fix to also return mode - } - // Check if mode changed - if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { - autonomy_mode = true; - } - - /* Right side longitudinal wheel set */ - // Forward - if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { - // map the duty from 0 to 1 given the min and max threshold values - right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) - { - right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); - right_dir = DIR_BW; - } - /* Left side longitudinal wheel set */ - // Forward - if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { - left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) - { - left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); - left_dir = DIR_BW; - } - - /* Define and fill wheel motor commands */ - Array wheel_motor_commands = {{ - {left_duty, RELEASE_BRAKE, left_dir}, - {right_duty, RELEASE_BRAKE, right_dir}, - }}; - - /* Return reference to the command set */ - return wheel_motor_commands; -} diff --git a/vehicle_interface/RC_with_US/rc_commands.h b/vehicle_interface/RC_with_US/rc_commands.h deleted file mode 100644 index c47125c0..00000000 --- a/vehicle_interface/RC_with_US/rc_commands.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RC_COMMANDS_H -#define RC_COMMANDS_H -#include "pinout.h" -#include "wheel.h" -#include "util.h" - -/* Define the max and min thresholds for the longitudinal wheel sets */ -// CH2 - right longitudinal wheel set -#define RC_RIGHT_SET_FW_MAX 1985 -#define RC_RIGHT_SET_FW_MIN 1638 -#define RC_RIGHT_SET_BW_MAX 1310 -#define RC_RIGHT_SET_BW_MIN 980 -// CH3 - left longitudinal wheel set -#define RC_LEFT_SET_FW_MAX 1972 -#define RC_LEFT_SET_FW_MIN 1632 -#define RC_LEFT_SET_BW_MAX 1320 -#define RC_LEFT_SET_BW_MIN 997//1007 -/* Define channels and thresholds for the switches */ -// CH5/6 - SWA/B -#define RC_SWX_LOW_MAX 1000//994 -#define RC_SWX_LOW_MIN 950//987 -#define RC_SWX_HIGH_MAX 2000//1981 -#define RC_SWX_HIGH_MIN 1900//1974 - -/* Function prototypes for rc commands */ -Array fetch_rc_commands(void); - -#endif // !RC_COMMANDS_H \ No newline at end of file diff --git a/vehicle_interface/RC_with_US/util.cpp b/vehicle_interface/RC_with_US/util.cpp deleted file mode 100644 index 17dc0b7a..00000000 --- a/vehicle_interface/RC_with_US/util.cpp +++ /dev/null @@ -1,14 +0,0 @@ -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} diff --git a/vehicle_interface/RC_with_US/util.h b/vehicle_interface/RC_with_US/util.h deleted file mode 100644 index 40a279e9..00000000 --- a/vehicle_interface/RC_with_US/util.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef UTIL_H -#define UTIL_H - -#include - -#define LN_SCALE_FACTOR -3.0 -#define MAX_DUTY 0.95 - -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); - -template -struct Array { - // Storage - T data[N]; - - static size_t length() { return N; } - using type = T; - - // Item access - T &operator[](size_t index) { return data[index]; } - const T &operator[](size_t index) const { return data[index]; } - - // Iterators - T *begin() { return &data[0]; } - const T *begin() const { return &data[0]; } - T *end() { return &data[N]; } - const T *end() const { return &data[N]; } - - // Comparisons - bool operator==(const Array &rhs) const { - if (this == &rhs) - return true; - for (size_t i = 0; i < N; i++) - if ((*this)[i] != rhs[i]) - return false; - return true; - } - bool operator!=(const Array &rhs) const { - return !(*this == rhs); - } -}; - -#endif // UTIL_H diff --git a/vehicle_interface/RC_with_US/wheel.h b/vehicle_interface/RC_with_US/wheel.h deleted file mode 100644 index 3a08dd4a..00000000 --- a/vehicle_interface/RC_with_US/wheel.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef Wheel_h -#define Wheel_h - -#include -#include "pinout.h" -#include "util.h" -#include "UCMotor.h" - -/* Define Important Values */ -#if defined(__AVR_ATmega328P__) - #define TOP 0XFF - #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)) - #define TOP 0x03FF // Top value of the registers (configuration done in MAIN) -#endif - - -/** - * @brief motor command structure that includes duty, brake, and direction - * - */ -typedef struct wheel_motor_command_t -{ - float duty_cycle; // float from 0 to 1 - bool brake_release; - bool dir; -}; - -// Direction values -#define DIR_FW true -#define DIR_BW false -#define ENGAGE_BRAKE false -#define RELEASE_BRAKE true - - -/* Else, define values for mini rc */ -#define NUM_WHEELS 2 // driving with only two drive signals - -typedef enum wheel_index_t -{ - Start = -1, - L = 0U, - R = 1U, - End = 2U, -} wheel_indices; - -const Array all_brake_command = {{ - {0.0, ENGAGE_BRAKE, FORWARD}, - {0.0, ENGAGE_BRAKE, FORWARD} -}}; - -#endif // !Wheel_h diff --git a/vehicle_interface/RC_with_US/README.md b/vehicle_interface/main/README.md similarity index 92% rename from vehicle_interface/RC_with_US/README.md rename to vehicle_interface/main/README.md index 40a6e4f3..711e98df 100644 --- a/vehicle_interface/RC_with_US/README.md +++ b/vehicle_interface/main/README.md @@ -1,4 +1,5 @@ # PropBot + An I2C library is necessary for this project. Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster @@ -7,4 +8,4 @@ And install the library onto Arduino IDE. For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries -Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car \ No newline at end of file +Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car diff --git a/vehicle_interface/main/SoftI2CMaster.h b/vehicle_interface/main/SoftI2CMaster.h new file mode 100644 index 00000000..68de1a9a --- /dev/null +++ b/vehicle_interface/main/SoftI2CMaster.h @@ -0,0 +1,862 @@ +/* Arduino SoftI2C library. + * + * Version 1.4 + * + * Copyright (C) 2013, Bernhard Nebel and Peter Fleury + * + * This is a very fast and very light-weight software I2C-master library + * written in assembler. It is based on Peter Fleury's I2C software + * library: http://homepage.hispeed.ch/peterfleury/avr-software.html + * Recently, the hardware implementation has been added to the code, + * which can be enabled by defining I2C_HARDWARE. + * + * This Library is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This Library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with the Arduino I2cMaster Library. If not, see + * . + */ + +/* In order to use the library, you need to define SDA_PIN, SCL_PIN, + * SDA_PORT and SCL_PORT before including this file. Have a look at + * http://www.arduino.cc/en/Reference/PortManipulation for finding out + * which values to use. For example, if you use digital pin 3 (corresponding + * to PD3) for SDA and digital pin 13 (corresponding to PB5) + * for SCL on a standard Arduino, + * you have to use the following definitions: + * #define SDA_PIN 3 + * #define SDA_PORT PORTD + * #define SCL_PIN 5 + * #define SCL_PORT PORTB + * + * Alternatively, you can define the compile time constant I2C_HARDWARE, + * in which case the TWI hardware is used. In this case you have to use + * the standard SDA/SCL pins (and, of course, the chip needs to support + * this). + * + * You can also define the following constants (see also below): + ' - I2C_PULLUP = 1 meaning that internal pullups should be used + * - I2C_CPUFREQ, when changing CPU clock frequency dynamically + * - I2C_FASTMODE = 1 meaning that the I2C bus allows speeds up to 400 kHz + * - I2C_SLOWMODE = 1 meaning that the I2C bus will allow only up to 25 kHz + * - I2C_NOINTERRUPT = 1 in order to prohibit interrupts while + * communicating (see below). This can be useful if you use the library + * for communicationg with SMbus devices, which have timeouts. + * Note, however, that interrupts are disabled from issuing a start condition + * until issuing a stop condition. So use this option with care! + * - I2C_TIMEOUT = 0..10000 msec in order to return from the I2C functions + * in case of a I2C bus lockup (i.e., SCL constantly low). 0 means no timeout. + * - I2C_MAXWAIT = 0..32767 number of retries in i2c_start_wait. 0 means never stop. + */ + +/* Changelog: + * Version 2.1 + * - added conditional to check whether it is the right MCU type + * Version 2.0 + * - added hardware support as well. + * Version 1.4: + * - added "maximum retry" in i2c_start_wait in order to avoid lockup + * - added "internal pullups", but be careful since the option stretches the I2C specs + * Version 1.3: + * - added "__attribute__ ((used))" for all functions declared with "__attribute__ ((noinline))" + * Now the module is also usable in Arduino 1.6.11+ + * Version 1.2: + * - added pragma to avoid "unused parameter warnings" (suggestion by Walter) + * - replaced wrong license file + * Version 1.1: + * - removed I2C_CLOCK_STRETCHING + * - added I2C_TIMEOUT time in msec (0..10000) until timeout or 0 if no timeout + * - changed i2c_init to return true iff both SDA and SCL are high + * - changed interrupt disabling so that the previous IRQ state is restored + * Version 1.0: basic functionality + */ + +#ifndef __AVR_ARCH__ +#error "Not an AVR MCU! Use 'SlowSoftI2CMaster' library instead of 'SoftI2CMaster'!" +#else + +#ifndef _SOFTI2C_H +#define _SOFTI2C_H 1 + +#include +#include +#include + +#pragma GCC diagnostic push + +#pragma GCC diagnostic ignored "-Wunused-parameter" + + + +// Init function. Needs to be called once in the beginning. +// Returns false if SDA or SCL are low, which probably means +// a I2C bus lockup or that the lines are not pulled up. +bool __attribute__ ((noinline)) i2c_init(void) __attribute__ ((used)); + +// Start transfer function: is the 8-bit I2C address (including the R/W +// bit). +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_start(uint8_t addr) __attribute__ ((used)); + +// Similar to start function, but wait for an ACK! Will timeout if I2C_MAXWAIT > 0. +bool __attribute__ ((noinline)) i2c_start_wait(uint8_t addr) __attribute__ ((used)); + +// Repeated start function: After having claimed the bus with a start condition, +// you can address another or the same chip again without an intervening +// stop condition. +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_rep_start(uint8_t addr) __attribute__ ((used)); + +// Issue a stop condition, freeing the bus. +void __attribute__ ((noinline)) i2c_stop(void) asm("ass_i2c_stop") __attribute__ ((used)); + +// Write one byte to the slave chip that had been addressed +// by the previous start call. is the byte to be sent. +// Return: true if the slave replies with an "acknowledge", false otherwise +bool __attribute__ ((noinline)) i2c_write(uint8_t value) asm("ass_i2c_write") __attribute__ ((used)); + + +// Read one byte. If is true, we send a NAK after having received +// the byte in order to terminate the read sequence. +uint8_t __attribute__ ((noinline)) i2c_read(bool last) __attribute__ ((used)); + +// If you want to use the TWI hardeware, you have to define I2C_HARDWARE to be 1 +#ifndef I2C_HARDWARE +#define I2C_HARDWARE 0 +#endif + +#if I2C_HARDWARE +#ifndef TWDR +#error This chip does not support hardware I2C. Please undfine I2C_HARDWARE +#endif +#endif + +// You can set I2C_CPUFREQ independently of F_CPU if you +// change the CPU frequency on the fly. If you do not define it, +// it will use the value of F_CPU +#ifndef I2C_CPUFREQ +#define I2C_CPUFREQ F_CPU +#endif + +// If I2C_FASTMODE is set to 1, then the highest possible frequency below 400kHz +// is selected. Be aware that not all slave chips may be able to deal with that! +#ifndef I2C_FASTMODE +#define I2C_FASTMODE 0 +#endif + +// If I2C_FASTMODE is not defined or defined to be 0, then you can set +// I2C_SLOWMODE to 1. In this case, the I2C frequency will not be higher +// than 25KHz. This could be useful for problematic buses with high pull-ups +// and high capasitance. +#ifndef I2C_SLOWMODE +#define I2C_SLOWMODE 0 +#endif + +// If I2C_PULLUP is set to 1, then the internal pull-up resistors are used. +// This does not conform with the I2C specs, since the bus lines will be +// temporarily in high-state and the internal resistors have roughly 50k. +// With low bus speeds und short buses it usually works, though (hopefully). +#ifndef I2C_PULLUP +#define I2C_PULLUP 0 +#endif + +// if I2C_NOINTERRUPT is 1, then the I2C routines are not interruptable. +// This is most probably only necessary if you are using a 1MHz system clock, +// you are communicating with a SMBus device, and you want to avoid timeouts. +// Be aware that the interrupt bit is enabled after each call. So the +// I2C functions should not be called in interrupt routines or critical regions. +#ifndef I2C_NOINTERRUPT +#define I2C_NOINTERRUPT 0 +#endif + +// I2C_TIMEOUT can be set to a value between 1 and 10000. +// If it is defined and nonzero, it leads to a timeout if the +// SCL is low longer than I2C_TIMEOUT milliseconds, i.e., max timeout is 10 sec +#ifndef I2C_TIMEOUT +#define I2C_TIMEOUT 0 +#else +#if I2C_TIMEOUT > 10000 +#error I2C_TIMEOUT is too large +#endif +#endif + +// I2C_MAXWAIT can be set to any value between 0 and 32767. 0 means no time out. +#ifndef I2C_MAXWAIT +#define I2C_MAXWAIT 500 +#else +#if I2C_MAXWAIT > 32767 || I2C_MAXWAIT < 0 +#error Illegal I2C_MAXWAIT value +#endif +#endif + +#define I2C_TIMEOUT_DELAY_LOOPS (I2C_CPUFREQ/1000UL)*I2C_TIMEOUT/4000UL +#if I2C_TIMEOUT_DELAY_LOOPS < 1 +#define I2C_MAX_STRETCH 1 +#else +#if I2C_TIMEOUT_DELAY_LOOPS > 60000UL +#define I2C_MAX_STRETCH 60000UL +#else +#define I2C_MAX_STRETCH I2C_TIMEOUT_DELAY_LOOPS +#endif +#endif + +#if I2C_FASTMODE +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/350000L)/2-18)/3) +#define SCL_CLOCK 400000UL +#else +#if I2C_SLOWMODE +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/23500L)/2-18)/3) +#define SCL_CLOCK 25000UL +#else +#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/90000L)/2-18)/3) +#define SCL_CLOCK 100000UL +#endif +#endif + +// constants for reading & writing +#define I2C_READ 1 +#define I2C_WRITE 0 + +#if !I2C_HARDWARE +// map the IO register back into the IO address space +#define SDA_DDR (_SFR_IO_ADDR(SDA_PORT) - 1) +#define SCL_DDR (_SFR_IO_ADDR(SCL_PORT) - 1) +#define SDA_OUT _SFR_IO_ADDR(SDA_PORT) +#define SCL_OUT _SFR_IO_ADDR(SCL_PORT) +#define SDA_IN (_SFR_IO_ADDR(SDA_PORT) - 2) +#define SCL_IN (_SFR_IO_ADDR(SCL_PORT) - 2) + +#ifndef __tmp_reg__ +#define __tmp_reg__ 0 +#endif + +// Internal delay functions. +void __attribute__ ((noinline)) i2c_delay_half(void) asm("ass_i2c_delay_half") __attribute__ ((used)); +void __attribute__ ((noinline)) i2c_wait_scl_high(void) asm("ass_i2c_wait_scl_high") __attribute__ ((used)); + +void i2c_delay_half(void) +{ // function call 3 cycles => 3C +#if I2C_DELAY_COUNTER < 1 + __asm__ __volatile__ (" ret"); + // 7 cycles for call and return +#else + __asm__ __volatile__ + ( + " ldi r25, %[DELAY] ;load delay constant ;; 4C \n\t" + "_Lidelay: \n\t" + " dec r25 ;decrement counter ;; 4C+xC \n\t" + " brne _Lidelay ;;5C+(x-1)2C+xC\n\t" + " ret ;; 9C+(x-1)2C+xC = 7C+xC" + : : [DELAY] "M" I2C_DELAY_COUNTER : "r25"); + // 7 cycles + 3 times x cycles +#endif +} + +void i2c_wait_scl_high(void) +{ +#if I2C_TIMEOUT <= 0 + __asm__ __volatile__ + ("_Li2c_wait_stretch: \n\t" + " sbis %[SCLIN],%[SCLPIN] ;wait for SCL high \n\t" + " rjmp _Li2c_wait_stretch \n\t" + " cln ;signal: no timeout \n\t" + " ret " + : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN)); +#else + __asm__ __volatile__ + ( " ldi r27, %[HISTRETCH] ;load delay counter \n\t" + " ldi r26, %[LOSTRETCH] \n\t" + "_Lwait_stretch: \n\t" + " clr __tmp_reg__ ;do next loop 255 times \n\t" + "_Lwait_stretch_inner_loop: \n\t" + " rcall _Lcheck_scl_level ;call check function ;; 12C \n\t" + " brpl _Lstretch_done ;done if N=0 ;; +1 = 13C\n\t" + " dec __tmp_reg__ ;dec inner loop counter;; +1 = 14C\n\t" + " brne _Lwait_stretch_inner_loop ;; +2 = 16C\n\t" + " sbiw r26,1 ;dec outer loop counter \n\t" + " brne _Lwait_stretch ;continue with outer loop \n\t" + " sen ;timeout -> set N-bit=1 \n\t" + " rjmp _Lwait_return ;and return with N=1\n\t" + "_Lstretch_done: ;SCL=1 sensed \n\t" + " cln ;OK -> clear N-bit \n\t" + " rjmp _Lwait_return ; and return with N=0 \n\t" + + "_Lcheck_scl_level: ;; call = 3C\n\t" + " cln ;; +1C = 4C \n\t" + " sbic %[SCLIN],%[SCLPIN] ;skip if SCL still low ;; +2C = 6C \n\t" + " rjmp _Lscl_high ;; +0C = 6C \n\t" + " sen ;; +1 = 7C\n\t " + "_Lscl_high: " + " nop ;; +1C = 8C \n\t" + " ret ;return N-Bit=1 if low ;; +4 = 12C\n\t" + + "_Lwait_return:" + : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), + [HISTRETCH] "M" (I2C_MAX_STRETCH>>8), + [LOSTRETCH] "M" (I2C_MAX_STRETCH&0xFF) + : "r26", "r27"); +#endif +} +#endif // !I2C_HARDWARE + +bool i2c_init(void) +#if I2C_HARDWARE +{ +#if I2C_PULLUP + digitalWrite(SDA, 1); + digitalWrite(SCL, 1); +#else + digitalWrite(SDA, 0); + digitalWrite(SCL, 0); +#endif +#if ((I2C_CPUFREQ/SCL_CLOCK)-16)/2 < 250 + TWSR = 0; /* no prescaler */ + TWBR = ((I2C_CPUFREQ/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ +#else + TWSR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_START) && (twst != TW_REP_START)) return false; + + // send device address + TWDR = addr; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return false; + + return true; +} +#else +{ + __asm__ __volatile__ + ( +#if I2C_NOINTERRUPT + " cli ;clear IRQ bit \n\t" +#endif + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;wait T/2 \n\t" + " rcall ass_i2c_write ;now write address \n\t" + " ret" + : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SCLIN] "I" (SCL_IN),[SCLPIN] "I" (SCL_PIN)); + return true; // we never return here! +} +#endif + +bool i2c_rep_start(uint8_t addr) +#if I2C_HARDWARE +{ + return i2c_start(addr); +} +#else +{ + __asm__ __volatile__ + + ( +#if I2C_NOINTERRUPT + " cli \n\t" +#endif +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable SDA pull-up\n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " rcall ass_i2c_write \n\t" + " ret" + : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), + [SCLIN] "I" (SCL_IN), [SCLOUT] "I" (SCL_OUT), [SDAOUT] "I" (SDA_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); + return true; // just to fool the compiler +} +#endif + +bool i2c_start_wait(uint8_t addr) +#if I2C_HARDWARE +{ + uint8_t twst; + uint16_t maxwait = I2C_MAXWAIT; +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + while (true) { + // send START condition + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst != TW_START) && (twst != TW_REP_START)) continue; + + // send device address + TWDR = addr; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits. + twst = TW_STATUS & 0xF8; + if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) + { + /* device busy, send stop condition to terminate write operation */ + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + if (maxwait) + if (--maxwait == 0) + return false; + + continue; + } + //if( twst != TW_MT_SLA_ACK) return 1; + return true; + } +} +#else +{ + __asm__ __volatile__ + ( + " push r24 ;save original parameter \n\t" +#if I2C_MAXWAIT + " ldi r31, %[HIMAXWAIT] ;load max wait counter \n\t" + " ldi r30, %[LOMAXWAIT] ;load low byte \n\t" +#endif + "_Li2c_start_wait1: \n\t" + " pop r24 ;restore original parameter\n\t" + " push r24 ;and save again \n\t" +#if I2C_NOINTERRUPT + " cli ;disable interrupts \n\t" +#endif + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;delay T/2 \n\t" + " rcall ass_i2c_write ;write address \n\t" + " tst r24 ;if device not busy -> done \n\t" + " brne _Li2c_start_wait_done \n\t" + " rcall ass_i2c_stop ;terminate write & enable IRQ \n\t" +#if I2C_MAXWAIT + " sbiw r30,1 ;decrement max wait counter\n\t" + " breq _Li2c_start_wait_done ;if zero reached, exit with false -> r24 already zero!\n\t" +#endif + " rjmp _Li2c_start_wait1 ;device busy, poll ack again \n\t" + "_Li2c_start_wait_done: \n\t" + " clr r25 ;clear high byte of return value\n\t" + " pop __tmp_reg__ ;pop off orig argument \n\t" + " ret " + : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAOUT] "I" (SDA_OUT), + [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), + [HIMAXWAIT] "M" (I2C_MAXWAIT>>8), + [LOMAXWAIT] "M" (I2C_MAXWAIT&0xFF) + : "r30", "r31" ); +} +#endif + +void i2c_stop(void) +#if I2C_HARDWARE +{ +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + /* send stop condition */ + TWCR = (1< I2C_TIMEOUT) return; +#endif + } +} +#else +{ + __asm__ __volatile__ + ( +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" + " rcall ass_i2c_delay_half ;T/2 delay \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " rcall ass_i2c_delay_half ;T/2 delay \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" + " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rcall ass_i2c_delay_half \n\t" +#if I2C_NOINTERRUPT + " sei ;enable interrupts again!\n\t" +#endif + : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); +} +#endif + + +bool i2c_write(uint8_t value) +#if I2C_HARDWARE +{ + uint8_t twst; +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + + // send data to the previously addressed device + TWDR = value; + TWCR = (1< I2C_TIMEOUT) return false; +#endif + } + + // check value of TWI Status Register. Mask prescaler bits + twst = TW_STATUS & 0xF8; + if( twst != TW_MT_DATA_ACK) return false; + return true; +} +#else +{ + __asm__ __volatile__ + ( + " sec ;set carry flag \n\t" + " rol r24 ;shift in carry and shift out MSB \n\t" + " rjmp _Li2c_write_first \n\t" + "_Li2c_write_bit:\n\t" + " lsl r24 ;left shift into carry ;; 1C\n\t" + "_Li2c_write_first:\n\t" + " breq _Li2c_get_ack ;jump if TXreg is empty;; +1 = 2C \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 4C \n\t" + " nop \n\t" + " nop \n\t" + " nop \n\t" + " brcc _Li2c_write_low ;;+1/+2=5/6C\n\t" + " nop ;; +1 = 7C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA ;; +2 = 9C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rjmp _Li2c_write_high ;; +2 = 11C \n\t" + "_Li2c_write_low: \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; +2 = 9C \n\t" + " rjmp _Li2c_write_high ;;+2 = 11C \n\t" + "_Li2c_write_high: \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;;+X = 11C+X\n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;;+2 = 13C+X\n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " cln ;clear N-bit ;;+1 = 14C+X\n\t" + " nop \n\t" + " nop \n\t" + " nop \n\t" + " sbis %[SCLIN],%[SCLPIN] ;check for SCL high ;;+2 = 16C+X\n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brpl _Ldelay_scl_high ;;+2 = 18C+X\n\t" + "_Li2c_write_return_false: \n\t" + " clr r24 ; return false because of timeout \n\t" + " rjmp _Li2c_write_return \n\t" + "_Ldelay_scl_high: \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;;+X= 18C+2X\n\t" +#endif + " rjmp _Li2c_write_bit \n\t" + " ;; +2 = 20C +2X for one bit-loop \n\t" + "_Li2c_get_ack: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 5C \n\t" + " nop \n\t" + " nop \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA ;;+2 = 7C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; +X = 7C+X \n\t" +#endif + " clr r25 ;; 17C+2X \n\t" + " clr r24 ;return 0 ;; 14C + X \n\t" + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; +2 = 9C+X\n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + "_Li2c_ack_wait: \n\t" + " cln ; clear N-bit ;; 10C + X\n\t" + " nop \n\t" + " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X \n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brmi _Li2c_write_return_false ;; 13C + X \n\t " + " sbis %[SDAIN],%[SDAPIN] ;if SDA hi -> return 0 ;; 15C + X \n\t" + " ldi r24,1 ;return true ;; 16C + X \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 16C + 2X \n\t" +#endif + "_Li2c_write_return: \n\t" + " nop \n\t " + " nop \n\t " +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" + " ret \n\t" + " ;; + 4 = 17C + 2X for acknowldge bit" + :: + [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN)); + return true; // fooling the compiler +} +#endif + +uint8_t i2c_read(bool last) +#if I2C_HARDWARE +{ +#if I2C_TIMEOUT + uint32_t start = millis(); +#endif + + TWCR = (1< I2C_TIMEOUT) return 0xFF; +#endif + } + return TWDR; +} +#else +{ + __asm__ __volatile__ + ( + " ldi r23,0x01 \n\t" + "_Li2c_read_bit: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA(prev. ACK);; 4C \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " nop \n\t" + " nop \n\t" + " nop \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 4C+X \n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 6C + X \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 6C + 2X \n\t" +#endif + " cln ; clear N-bit ;; 7C + 2X \n\t" + " nop \n\t " + " nop \n\t " + " nop \n\t " + " sbis %[SCLIN], %[SCLPIN] ;check for SCL high ;; 9C +2X \n\t" + " rcall ass_i2c_wait_scl_high \n\t" + " brmi _Li2c_read_return ;return if timeout ;; 10C + 2X\n\t" + " clc ;clear carry flag ;; 11C + 2X\n\t" + " sbic %[SDAIN],%[SDAPIN] ;if SDA is high ;; 11C + 2X\n\t" + " sec ;set carry flag ;; 12C + 2X\n\t" + " rol r23 ;store bit ;; 13C + 2X\n\t" + " brcc _Li2c_read_bit ;while receiv reg not full \n\t" + " ;; 15C + 2X for one bit loop \n\t" + + "_Li2c_put_ack: \n\t" +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" + " cpi r24,0 ;; 3C \n\t" + " breq _Li2c_put_ack_low ;if (ack=0) ;; 5C \n\t" + " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" +#if I2C_PULLUP + " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" +#endif + " rjmp _Li2c_put_ack_high \n\t" + "_Li2c_put_ack_low: ;else \n\t" +#if I2C_PULLUP + " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" +#endif + " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; 7C \n\t" + "_Li2c_put_ack_high: \n\t" + " nop \n\t " + " nop \n\t " + " nop \n\t " +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 7C + X \n\t" +#endif + " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 9C +X \n\t" +#if I2C_PULLUP + " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" +#endif + " cln ;clear N ;; +1 = 10C\n\t" + " nop \n\t " + " nop \n\t " + " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X\n\t" + " rcall ass_i2c_wait_scl_high \n\t" +#if I2C_DELAY_COUNTER >= 1 + " rcall ass_i2c_delay_half ;delay T/2 ;; 11C + 2X\n\t" +#endif + "_Li2c_read_return: \n\t" + " nop \n\t " + " nop \n\t " +#if I2C_PULLUP + " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" +#endif + "sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" + " mov r24,r23 ;; 12C + 2X \n\t" + " clr r25 ;; 13 C + 2X\n\t" + " ret ;; 17C + X" + :: + [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), + [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), + [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN) + ); + return ' '; // fool the compiler! +} +#endif + +#pragma GCC diagnostic pop + +#endif +#endif diff --git a/vehicle_interface/main/UCMotor.cpp b/vehicle_interface/main/UCMotor.cpp index d3e6501a..3978fc0e 100644 --- a/vehicle_interface/main/UCMotor.cpp +++ b/vehicle_interface/main/UCMotor.cpp @@ -1,729 +1,729 @@ -/** - * @file UCMotor.cpp - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ - -// UCTRONOICS Motor shield library -// this code is public domain, enjoy! - - -#if (ARDUINO >= 100) - #include "Arduino.h" -#else - #if defined(__AVR__) - #include - #endif - #include "WProgram.h" -#endif - -#include "UCMotor.h" - - - -static uint8_t latch_state; - -#if (MICROSTEPS == 8) -uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; -#elif (MICROSTEPS == 16) -uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; -#endif - -UCMotorController::UCMotorController(void) { - TimerInitalized = false; -} - -void UCMotorController::enable(void) { - // setup the latch - /* - LATCH_DDR |= _BV(LATCH); - ENABLE_DDR |= _BV(ENABLE); - CLK_DDR |= _BV(CLK); - SER_DDR |= _BV(SER); - */ - pinMode(MOTORLATCH, OUTPUT); - pinMode(MOTORENABLE, OUTPUT); - pinMode(MOTORDATA, OUTPUT); - pinMode(MOTORCLK, OUTPUT); - - latch_state = 0; - - latch_tx(); // "reset" - - //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! - digitalWrite(MOTORENABLE, LOW); -} - - -void UCMotorController::latch_tx(void) { - uint8_t i; - - //LATCH_PORT &= ~_BV(LATCH); - digitalWrite(MOTORLATCH, LOW); - - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - - for (i=0; i<8; i++) { - //CLK_PORT &= ~_BV(CLK); - digitalWrite(MOTORCLK, LOW); - - if (latch_state & _BV(7-i)) { - //SER_PORT |= _BV(SER); - digitalWrite(MOTORDATA, HIGH); - } else { - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - } - //CLK_PORT |= _BV(CLK); - digitalWrite(MOTORCLK, HIGH); - } - //LATCH_PORT |= _BV(LATCH); - digitalWrite(MOTORLATCH, HIGH); -} - -static UCMotorController MC; - -/****************************************** - MOTORS -******************************************/ -inline void initPWM1(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a - TCCR2B = freq & 0x7; - OCR2A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a - TCCR1B = (freq & 0x7) | _BV(WGM12); - OCR1A = 0; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(9, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase - OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC4RS = 0x0000; - OC4R = 0x0000; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(10, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase - OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC5RS = 0x0000; - OC5R = 0x0000; - #else - // If we are not using PWM for pin 11, then just do digital - digitalWrite(11, LOW); - #endif -#else - #error "This chip is not supported!" -#endif - #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) - pinMode(11, OUTPUT); - //Serial .println("OK"); - #endif -} - -inline void setPWM1(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR1A = s; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 - OC4RS = s; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 - OC5RS = s; - #else - // If we are not doing PWM output for M1, then just use on/off - if (s > 127) - { - digitalWrite(11, HIGH); - } - else - { - digitalWrite(11, LOW); - } - #endif -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM2(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2B (pin 3) - TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b - TCCR2B = freq & 0x7; - OCR2B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 3 is now PE5 (OC3C) - TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c - TCCR3B = (freq & 0x7) | _BV(WGM12); - OCR3C = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase - OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC1RS = 0x0000; - OC1R = 0x0000; -#else - #error "This chip is not supported!" -#endif - - pinMode(3, OUTPUT); -} - -inline void setPWM2(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR3C = s; -#elif defined(__PIC32MX__) - // Set the OC1 (pin3) PMW duty cycle from 0 to 255 - OC1RS = s; -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM3(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A / PD6 (pin 6) - TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A - //TCCR0B = freq & 0x7; - OCR0A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a - TCCR4B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR4A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase - OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC3RS = 0x0000; - OC3R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(6, OUTPUT); -} - -inline void setPWM3(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR4A = s; -#elif defined(__PIC32MX__) - // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 - OC3RS = s; -#else - #error "This chip is not supported!" -#endif -} - - - -inline void initPWM4(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0B / PD5 (pin 5) - TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a - //TCCR0B = freq & 0x7; - OCR0B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 5 is now PE3 (OC3A) - TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a - TCCR3B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR3A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase - OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC2RS = 0x0000; - OC2R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(5, OUTPUT); -} - -inline void setPWM4(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR3A = s; -#elif defined(__PIC32MX__) - // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 - OC2RS = s; -#else - #error "This chip is not supported!" -#endif -} - -UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { - motornum = num; - pwmfreq = freq; - MC.enable(); - - switch (num) { - case 1: - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM1(freq); - break; - case 2: - latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM2(freq); - break; - case 3: - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM3(freq); - break; - case 4: - latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM4(freq); - break; - } -} - -void UC_DCMotor::run(uint8_t cmd) { - uint8_t a, b; - switch (motornum) { - case 1: - a = MOTOR1_A; b = MOTOR1_B; break; - case 2: - a = MOTOR2_A; b = MOTOR2_B; break; - case 3: - a = MOTOR3_A; b = MOTOR3_B; break; - case 4: - a = MOTOR4_A; b = MOTOR4_B; break; - default: - return; - } - - switch (cmd) { - case FORWARD: - if(motornum ==3 || motornum ==4){ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - }else{ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - } - MC.latch_tx(); - break; - case BACKWARD: - if(motornum ==3 || motornum ==4){ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - }else{ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - } - MC.latch_tx(); - break; - case LEFT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx(); - } - break; - case RIGHT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - } - break; - case STOP: - latch_state &= ~_BV(a); // A and B both low - latch_state &= ~_BV(b); - MC.latch_tx(); - break; - } -} - -void UC_DCMotor::setSpeed(uint8_t speed) { - switch (motornum) { - case 1: - setPWM1(speed); break; - case 2: - setPWM2(speed); break; - case 3: - setPWM3(speed); break; - case 4: - setPWM4(speed); break; - } -} - -/****************************************** - STEPPERS -******************************************/ - -UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { - MC.enable(); - - revsteps = steps; - steppernum = num; - currentstep = 0; - - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(11, OUTPUT); - pinMode(3, OUTPUT); - digitalWrite(11, HIGH); - digitalWrite(3, HIGH); - - // use PWM for microstepping support - initPWM1(STEPPER1_PWM_RATE); - initPWM2(STEPPER1_PWM_RATE); - setPWM1(255); - setPWM2(255); - - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(5, HIGH); - digitalWrite(6, HIGH); - - // use PWM for microstepping support - // use PWM for microstepping support - initPWM3(STEPPER2_PWM_RATE); - initPWM4(STEPPER2_PWM_RATE); - setPWM3(255); - setPWM4(255); - } -} - -void UC_Stepper::setSpeed(uint16_t rpm) { - usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); - steppingcounter = 0; -} - -void UC_Stepper::release(void) { - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - } -} - -void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { - uint32_t uspers = usperstep; - uint8_t ret = 0; - - if (style == INTERLEAVE) { - uspers /= 2; - } - else if (style == MICROSTEP) { - uspers /= MICROSTEPS; - steps *= MICROSTEPS; -#ifdef MOTORDEBUG - Serial.print("steps = "); Serial.println(steps, DEC); -#endif - } - - while (steps--) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - if (style == MICROSTEP) { - while ((ret != 0) && (ret != MICROSTEPS)) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - } -} - -uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { - uint8_t a, b, c, d; - uint8_t ocrb, ocra; - - ocra = ocrb = 255; - - if (steppernum == 1) { - a = _BV(MOTOR1_A); - b = _BV(MOTOR2_A); - c = _BV(MOTOR1_B); - d = _BV(MOTOR2_B); - } else if (steppernum == 2) { - a = _BV(MOTOR3_A); - b = _BV(MOTOR4_A); - c = _BV(MOTOR3_B); - d = _BV(MOTOR4_B); - } else { - return 0; - } - - // next determine what sort of stepping procedure we're up to - if (style == SINGLE) { - if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } - else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next even step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } - else { - currentstep -= MICROSTEPS; - } - } - } else if (style == DOUBLE) { - if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next odd step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } else { - currentstep -= MICROSTEPS; - } - } - } else if (style == INTERLEAVE) { - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } - - if (style == MICROSTEP) { - if (dir == FORWARD) { - currentstep++; - } else { - // BACKWARDS - currentstep--; - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - - ocra = ocrb = 0; - if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { - ocra = microstepcurve[MICROSTEPS - currentstep]; - ocrb = microstepcurve[currentstep]; - } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { - ocra = microstepcurve[currentstep - MICROSTEPS]; - ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; - } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { - ocra = microstepcurve[MICROSTEPS*3 - currentstep]; - ocrb = microstepcurve[currentstep - MICROSTEPS*2]; - } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { - ocra = microstepcurve[currentstep - MICROSTEPS*3]; - ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; - } - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - -#ifdef MOTORDEBUG - Serial.print("current step: "); Serial.println(currentstep, DEC); - Serial.print(" pwmA = "); Serial.print(ocra, DEC); - Serial.print(" pwmB = "); Serial.println(ocrb, DEC); -#endif - - if (steppernum == 1) { - setPWM1(ocra); - setPWM2(ocrb); - } else if (steppernum == 2) { - setPWM3(ocra); - setPWM4(ocrb); - } - - - // release all - latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 - - //Serial.println(step, DEC); - if (style == MICROSTEP) { - if ((currentstep >= 0) && (currentstep < MICROSTEPS)) - latch_state |= a | b; - if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) - latch_state |= b | c; - if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) - latch_state |= c | d; - if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) - latch_state |= d | a; - } else { - switch (currentstep/(MICROSTEPS/2)) { - case 0: - latch_state |= a; // energize coil 1 only - break; - case 1: - latch_state |= a | b; // energize coil 1+2 - break; - case 2: - latch_state |= b; // energize coil 2 only - break; - case 3: - latch_state |= b | c; // energize coil 2+3 - break; - case 4: - latch_state |= c; // energize coil 3 only - break; - case 5: - latch_state |= c | d; // energize coil 3+4 - break; - case 6: - latch_state |= d; // energize coil 4 only - break; - case 7: - latch_state |= d | a; // energize coil 1+4 - break; - } - } - - - MC.latch_tx(); - return currentstep; -} - +/** + * @file UCMotor.cpp + * @author + * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + * + * @note No edits have been made to to this file aside from the brief + * + */ + +// UCTRONOICS Motor shield library +// this code is public domain, enjoy! + + +#if (ARDUINO >= 100) + #include "Arduino.h" +#else + #if defined(__AVR__) + #include + #endif + #include "WProgram.h" +#endif + +#include "UCMotor.h" + + + +static uint8_t latch_state; + +#if (MICROSTEPS == 8) +uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; +#elif (MICROSTEPS == 16) +uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; +#endif + +UCMotorController::UCMotorController(void) { + TimerInitalized = false; +} + +void UCMotorController::enable(void) { + // setup the latch + /* + LATCH_DDR |= _BV(LATCH); + ENABLE_DDR |= _BV(ENABLE); + CLK_DDR |= _BV(CLK); + SER_DDR |= _BV(SER); + */ + pinMode(MOTORLATCH, OUTPUT); + pinMode(MOTORENABLE, OUTPUT); + pinMode(MOTORDATA, OUTPUT); + pinMode(MOTORCLK, OUTPUT); + + latch_state = 0; + + latch_tx(); // "reset" + + //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! + digitalWrite(MOTORENABLE, LOW); +} + + +void UCMotorController::latch_tx(void) { + uint8_t i; + + //LATCH_PORT &= ~_BV(LATCH); + digitalWrite(MOTORLATCH, LOW); + + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + + for (i=0; i<8; i++) { + //CLK_PORT &= ~_BV(CLK); + digitalWrite(MOTORCLK, LOW); + + if (latch_state & _BV(7-i)) { + //SER_PORT |= _BV(SER); + digitalWrite(MOTORDATA, HIGH); + } else { + //SER_PORT &= ~_BV(SER); + digitalWrite(MOTORDATA, LOW); + } + //CLK_PORT |= _BV(CLK); + digitalWrite(MOTORCLK, HIGH); + } + //LATCH_PORT |= _BV(LATCH); + digitalWrite(MOTORLATCH, HIGH); +} + +static UCMotorController MC; + +/****************************************** + MOTORS +******************************************/ +inline void initPWM1(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a + TCCR2B = freq & 0x7; + OCR2A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a + TCCR1B = (freq & 0x7) | _BV(WGM12); + OCR1A = 0; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(9, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase + OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC4RS = 0x0000; + OC4R = 0x0000; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Make sure that pin 11 is an input, since we have tied together 9 and 11 + pinMode(10, OUTPUT); + pinMode(11, INPUT); + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase + OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC5RS = 0x0000; + OC5R = 0x0000; + #else + // If we are not using PWM for pin 11, then just do digital + digitalWrite(11, LOW); + #endif +#else + #error "This chip is not supported!" +#endif + #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) + pinMode(11, OUTPUT); + //Serial .println("OK"); + #endif +} + +inline void setPWM1(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR1A = s; +#elif defined(__PIC32MX__) + #if defined(PIC32_USE_PIN9_FOR_M1_PWM) + // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 + OC4RS = s; + #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) + // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 + OC5RS = s; + #else + // If we are not doing PWM output for M1, then just use on/off + if (s > 127) + { + digitalWrite(11, HIGH); + } + else + { + digitalWrite(11, LOW); + } + #endif +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM2(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2B (pin 3) + TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b + TCCR2B = freq & 0x7; + OCR2B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 3 is now PE5 (OC3C) + TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c + TCCR3B = (freq & 0x7) | _BV(WGM12); + OCR3C = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase + OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC1RS = 0x0000; + OC1R = 0x0000; +#else + #error "This chip is not supported!" +#endif + + pinMode(3, OUTPUT); +} + +inline void setPWM2(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer2A on PB3 (Arduino pin #11) + OCR2B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 11 is now PB5 (OC1A) + OCR3C = s; +#elif defined(__PIC32MX__) + // Set the OC1 (pin3) PMW duty cycle from 0 to 255 + OC1RS = s; +#else + #error "This chip is not supported!" +#endif +} + +inline void initPWM3(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A / PD6 (pin 6) + TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A + //TCCR0B = freq & 0x7; + OCR0A = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a + TCCR4B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR4A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase + OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC3RS = 0x0000; + OC3R = 0x0000; +#else + #error "This chip is not supported!" +#endif + pinMode(6, OUTPUT); +} + +inline void setPWM3(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0A = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR4A = s; +#elif defined(__PIC32MX__) + // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 + OC3RS = s; +#else + #error "This chip is not supported!" +#endif +} + + + +inline void initPWM4(uint8_t freq) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0B / PD5 (pin 5) + TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a + //TCCR0B = freq & 0x7; + OCR0B = 0; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 5 is now PE3 (OC3A) + TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a + TCCR3B = (freq & 0x7) | _BV(WGM12); + //TCCR4B = 1 | _BV(WGM12); + OCR3A = 0; +#elif defined(__PIC32MX__) + if (!MC.TimerInitalized) + { // Set up Timer2 for 80MHz counting fro 0 to 256 + T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 + TMR2 = 0x0000; + PR2 = 0x0100; + MC.TimerInitalized = true; + } + // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase + OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 + OC2RS = 0x0000; + OC2R = 0x0000; +#else + #error "This chip is not supported!" +#endif + pinMode(5, OUTPUT); +} + +inline void setPWM4(uint8_t s) { +#if defined(__AVR_ATmega8__) || \ + defined(__AVR_ATmega48__) || \ + defined(__AVR_ATmega88__) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega328P__) + // use PWM from timer0A on PB3 (Arduino pin #6) + OCR0B = s; +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + // on arduino mega, pin 6 is now PH3 (OC4A) + OCR3A = s; +#elif defined(__PIC32MX__) + // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 + OC2RS = s; +#else + #error "This chip is not supported!" +#endif +} + +UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { + motornum = num; + pwmfreq = freq; + MC.enable(); + + switch (num) { + case 1: + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM1(freq); + break; + case 2: + latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM2(freq); + break; + case 3: + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM3(freq); + break; + case 4: + latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 + MC.latch_tx(); + initPWM4(freq); + break; + } +} + +void UC_DCMotor::run(uint8_t cmd) { + uint8_t a, b; + switch (motornum) { + case 1: + a = MOTOR1_A; b = MOTOR1_B; break; + case 2: + a = MOTOR2_A; b = MOTOR2_B; break; + case 3: + a = MOTOR3_A; b = MOTOR3_B; break; + case 4: + a = MOTOR4_A; b = MOTOR4_B; break; + default: + return; + } + + switch (cmd) { + case FORWARD: + if(motornum ==3 || motornum ==4){ + latch_state |= _BV(a); + latch_state &= ~_BV(b); + }else{ + latch_state &= ~_BV(a); + latch_state |= _BV(b); + } + MC.latch_tx(); + break; + case BACKWARD: + if(motornum ==3 || motornum ==4){ + latch_state &= ~_BV(a); + latch_state |= _BV(b); + }else{ + latch_state |= _BV(a); + latch_state &= ~_BV(b); + } + MC.latch_tx(); + break; + case LEFT: + if(motornum ==3){ + a = MOTOR3_A; b = MOTOR3_B; //backward + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();} + else if(motornum ==4){ + a = MOTOR4_A; b = MOTOR4_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + }else if(motornum ==1){ + a = MOTOR1_A; b = MOTOR1_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + }else if(motornum ==2){ + a = MOTOR2_A; b = MOTOR2_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx(); + } + break; + case RIGHT: + if(motornum ==3){ + a = MOTOR3_A; b = MOTOR3_B; //backward + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx();} + else if(motornum ==4){ + a = MOTOR4_A; b = MOTOR4_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();}else if(motornum ==1){ + a = MOTOR1_A; b = MOTOR1_B; + latch_state &= ~_BV(a); + latch_state |= _BV(b); + MC.latch_tx();}else if(motornum ==2){ + a = MOTOR2_A; b = MOTOR2_B; + latch_state |= _BV(a); + latch_state &= ~_BV(b); + MC.latch_tx(); + } + break; + case STOP: + latch_state &= ~_BV(a); // A and B both low + latch_state &= ~_BV(b); + MC.latch_tx(); + break; + } +} + +void UC_DCMotor::setSpeed(uint8_t speed) { + switch (motornum) { + case 1: + setPWM1(speed); break; + case 2: + setPWM2(speed); break; + case 3: + setPWM3(speed); break; + case 4: + setPWM4(speed); break; + } +} + +/****************************************** + STEPPERS +******************************************/ + +UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { + MC.enable(); + + revsteps = steps; + steppernum = num; + currentstep = 0; + + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(11, OUTPUT); + pinMode(3, OUTPUT); + digitalWrite(11, HIGH); + digitalWrite(3, HIGH); + + // use PWM for microstepping support + initPWM1(STEPPER1_PWM_RATE); + initPWM2(STEPPER1_PWM_RATE); + setPWM1(255); + setPWM2(255); + + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + + // enable both H bridges + pinMode(5, OUTPUT); + pinMode(6, OUTPUT); + digitalWrite(5, HIGH); + digitalWrite(6, HIGH); + + // use PWM for microstepping support + // use PWM for microstepping support + initPWM3(STEPPER2_PWM_RATE); + initPWM4(STEPPER2_PWM_RATE); + setPWM3(255); + setPWM4(255); + } +} + +void UC_Stepper::setSpeed(uint16_t rpm) { + usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); + steppingcounter = 0; +} + +void UC_Stepper::release(void) { + if (steppernum == 1) { + latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & + ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 + MC.latch_tx(); + } else if (steppernum == 2) { + latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & + ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 + MC.latch_tx(); + } +} + +void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { + uint32_t uspers = usperstep; + uint8_t ret = 0; + + if (style == INTERLEAVE) { + uspers /= 2; + } + else if (style == MICROSTEP) { + uspers /= MICROSTEPS; + steps *= MICROSTEPS; +#ifdef MOTORDEBUG + Serial.print("steps = "); Serial.println(steps, DEC); +#endif + } + + while (steps--) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + if (style == MICROSTEP) { + while ((ret != 0) && (ret != MICROSTEPS)) { + ret = onestep(dir, style); + delay(uspers/1000); // in ms + steppingcounter += (uspers % 1000); + if (steppingcounter >= 1000) { + delay(1); + steppingcounter -= 1000; + } + } + } +} + +uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { + uint8_t a, b, c, d; + uint8_t ocrb, ocra; + + ocra = ocrb = 255; + + if (steppernum == 1) { + a = _BV(MOTOR1_A); + b = _BV(MOTOR2_A); + c = _BV(MOTOR1_B); + d = _BV(MOTOR2_B); + } else if (steppernum == 2) { + a = _BV(MOTOR3_A); + b = _BV(MOTOR4_A); + c = _BV(MOTOR3_B); + d = _BV(MOTOR4_B); + } else { + return 0; + } + + // next determine what sort of stepping procedure we're up to + if (style == SINGLE) { + if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } + else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next even step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } + else { + currentstep -= MICROSTEPS; + } + } + } else if (style == DOUBLE) { + if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } else { // go to the next odd step + if (dir == FORWARD) { + currentstep += MICROSTEPS; + } else { + currentstep -= MICROSTEPS; + } + } + } else if (style == INTERLEAVE) { + if (dir == FORWARD) { + currentstep += MICROSTEPS/2; + } else { + currentstep -= MICROSTEPS/2; + } + } + + if (style == MICROSTEP) { + if (dir == FORWARD) { + currentstep++; + } else { + // BACKWARDS + currentstep--; + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + + ocra = ocrb = 0; + if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { + ocra = microstepcurve[MICROSTEPS - currentstep]; + ocrb = microstepcurve[currentstep]; + } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { + ocra = microstepcurve[currentstep - MICROSTEPS]; + ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; + } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { + ocra = microstepcurve[MICROSTEPS*3 - currentstep]; + ocrb = microstepcurve[currentstep - MICROSTEPS*2]; + } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { + ocra = microstepcurve[currentstep - MICROSTEPS*3]; + ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; + } + } + + currentstep += MICROSTEPS*4; + currentstep %= MICROSTEPS*4; + +#ifdef MOTORDEBUG + Serial.print("current step: "); Serial.println(currentstep, DEC); + Serial.print(" pwmA = "); Serial.print(ocra, DEC); + Serial.print(" pwmB = "); Serial.println(ocrb, DEC); +#endif + + if (steppernum == 1) { + setPWM1(ocra); + setPWM2(ocrb); + } else if (steppernum == 2) { + setPWM3(ocra); + setPWM4(ocrb); + } + + + // release all + latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 + + //Serial.println(step, DEC); + if (style == MICROSTEP) { + if ((currentstep >= 0) && (currentstep < MICROSTEPS)) + latch_state |= a | b; + if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) + latch_state |= b | c; + if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) + latch_state |= c | d; + if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) + latch_state |= d | a; + } else { + switch (currentstep/(MICROSTEPS/2)) { + case 0: + latch_state |= a; // energize coil 1 only + break; + case 1: + latch_state |= a | b; // energize coil 1+2 + break; + case 2: + latch_state |= b; // energize coil 2 only + break; + case 3: + latch_state |= b | c; // energize coil 2+3 + break; + case 4: + latch_state |= c; // energize coil 3 only + break; + case 5: + latch_state |= c | d; // energize coil 3+4 + break; + case 6: + latch_state |= d; // energize coil 4 only + break; + case 7: + latch_state |= d | a; // energize coil 1+4 + break; + } + } + + + MC.latch_tx(); + return currentstep; +} + diff --git a/vehicle_interface/main/UCMotor.h b/vehicle_interface/main/UCMotor.h index 1e2f1e05..d90ca4ae 100644 --- a/vehicle_interface/main/UCMotor.h +++ b/vehicle_interface/main/UCMotor.h @@ -1,195 +1,195 @@ -/** - * @file UCMotor.h - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ -// UCTRONICS Motor shield library -// this code is public domain, enjoy! - -/* - * Usage Notes: - * For PIC32, all features work properly with the following two exceptions: - * - * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 - * to completely operate (four for motor outputs and two for RC servos), the - * M1 motor output will not have PWM ability when used with a PIC32 board. - * However, there is a very simple workaround. If you need to drive a stepper - * or DC motor with PWM on motor output M1, you can use the PWM output on pin - * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for - * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting - * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the - * two #defines below to activate the PWM on either pin 9 or pin 10. You will - * then have a fully functional microstepping for 2 stepper motors, or four - * DC motor outputs with PWM. - * - * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and - * the operation of DC motors and stepper motors as of 9/2012. This issue - * will get fixed in future MPIDE releases, but at the present time it means - * that the Motor Party example will NOT work properly. Any time you attach - * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will - * stop working. Thus no steppers or DC motors. - * - */ -// 09/15/2012 Modified for use with chipKIT boards - - -#ifndef _UCMotor_h_ -#define _UCMotor_h_ - -#include -#if defined(__AVR__) - #include - - //#define MOTORDEBUG 1 - - #define MICROSTEPS 16 // 8 or 16 - - #define MOTOR12_64KHZ _BV(CS20) // no prescale - #define MOTOR12_8KHZ _BV(CS21) // divide by 8 - #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 - #define MOTOR12_1KHZ _BV(CS22) // divide by 64 - - #define MOTOR34_64KHZ _BV(CS00) // no prescale - #define MOTOR34_8KHZ _BV(CS01) // divide by 8 - #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 - - #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors - #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 - #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 - -#elif defined(__PIC32MX__) - //#define MOTORDEBUG 1 - - // Uncomment the one of following lines if you have put a jumper from - // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. - // Either will enable PWM for M1 - //#define PIC32_USE_PIN9_FOR_M1_PWM - //#define PIC32_USE_PIN10_FOR_M1_PWM - - #define MICROSTEPS 16 // 8 or 16 - - // For PIC32 Timers, define prescale settings by PWM frequency - #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz - - #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz - - // PWM rate for DC motors. - #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ - // Note: for PIC32, both of these must be set to the same value - // since there's only one timebase for all 4 PWM outputs - #define STEPPER1_PWM_RATE MOTOR12_39KHZ - #define STEPPER2_PWM_RATE MOTOR34_39KHZ - -#endif - -// Bit positions in the 74HCT595 shift register output -#define MOTOR1_A 2 -#define MOTOR1_B 3 -#define MOTOR2_A 1 -#define MOTOR2_B 4 -#define MOTOR4_A 0 -#define MOTOR4_B 6 -#define MOTOR3_A 5 -#define MOTOR3_B 7 - -// Constants that the user passes in to the motor calls -#define FORWARD 1 -#define BACKWARD 2 -#define LEFT 3 -#define RIGHT 4 -#define STOP 5 - -#define FORWARD2 2 -#define BACKWARD2 1 - -#define BRAKE 3 -#define RELEASE 4 - -// Constants that the user passes in to the stepper calls -#define SINGLE 1 -#define DOUBLE 2 -#define INTERLEAVE 3 -#define MICROSTEP 4 - -/* -#define LATCH 4 -#define LATCH_DDR DDRB -#define LATCH_PORT PORTB - -#define CLK_PORT PORTD -#define CLK_DDR DDRD -#define CLK 4 - -#define ENABLE_PORT PORTD -#define ENABLE_DDR DDRD -#define ENABLE 7 - -#define SER 0 -#define SER_DDR DDRB -#define SER_PORT PORTB -*/ - -// Arduino pin names for interface to 74HCT595 latch -#define MOTORLATCH 12 -#define MOTORCLK 4 -#define MOTORENABLE 7 -#define MOTORDATA 8 - -class UCMotorController -{ - public: - UCMotorController(void); - void enable(void); - friend class UC_DCMotor; - void latch_tx(void); - uint8_t TimerInitalized; -}; - -class UC_DCMotor -{ - public: - UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); - void run(uint8_t); - void setSpeed(uint8_t); - - private: - uint8_t motornum, pwmfreq; -}; - -class UC_Stepper { - public: - UC_Stepper(uint16_t, uint8_t); - void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); - void setSpeed(uint16_t); - uint8_t onestep(uint8_t dir, uint8_t style); - void release(void); - uint16_t revsteps; // # steps per revolution - uint8_t steppernum; - uint32_t usperstep, steppingcounter; - private: - uint8_t currentstep; - -}; - -uint8_t getlatchstate(void); - -#endif +/** + * @file UCMotor.h + * @author + * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + * + * @note No edits have been made to to this file aside from the brief + * + */ +// UCTRONICS Motor shield library +// this code is public domain, enjoy! + +/* + * Usage Notes: + * For PIC32, all features work properly with the following two exceptions: + * + * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 + * to completely operate (four for motor outputs and two for RC servos), the + * M1 motor output will not have PWM ability when used with a PIC32 board. + * However, there is a very simple workaround. If you need to drive a stepper + * or DC motor with PWM on motor output M1, you can use the PWM output on pin + * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for + * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting + * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the + * two #defines below to activate the PWM on either pin 9 or pin 10. You will + * then have a fully functional microstepping for 2 stepper motors, or four + * DC motor outputs with PWM. + * + * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and + * the operation of DC motors and stepper motors as of 9/2012. This issue + * will get fixed in future MPIDE releases, but at the present time it means + * that the Motor Party example will NOT work properly. Any time you attach + * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will + * stop working. Thus no steppers or DC motors. + * + */ +// 09/15/2012 Modified for use with chipKIT boards + + +#ifndef _UCMotor_h_ +#define _UCMotor_h_ + +#include +#if defined(__AVR__) + #include + + //#define MOTORDEBUG 1 + + #define MICROSTEPS 16 // 8 or 16 + + #define MOTOR12_64KHZ _BV(CS20) // no prescale + #define MOTOR12_8KHZ _BV(CS21) // divide by 8 + #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 + #define MOTOR12_1KHZ _BV(CS22) // divide by 64 + + #define MOTOR34_64KHZ _BV(CS00) // no prescale + #define MOTOR34_8KHZ _BV(CS01) // divide by 8 + #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 + + #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors + #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 + #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 + +#elif defined(__PIC32MX__) + //#define MOTORDEBUG 1 + + // Uncomment the one of following lines if you have put a jumper from + // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. + // Either will enable PWM for M1 + //#define PIC32_USE_PIN9_FOR_M1_PWM + //#define PIC32_USE_PIN10_FOR_M1_PWM + + #define MICROSTEPS 16 // 8 or 16 + + // For PIC32 Timers, define prescale settings by PWM frequency + #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz + + #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz + #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz + #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz + #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz + #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz + #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz + #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz + #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz + #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz + + // PWM rate for DC motors. + #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ + // Note: for PIC32, both of these must be set to the same value + // since there's only one timebase for all 4 PWM outputs + #define STEPPER1_PWM_RATE MOTOR12_39KHZ + #define STEPPER2_PWM_RATE MOTOR34_39KHZ + +#endif + +// Bit positions in the 74HCT595 shift register output +#define MOTOR1_A 2 +#define MOTOR1_B 3 +#define MOTOR2_A 1 +#define MOTOR2_B 4 +#define MOTOR4_A 0 +#define MOTOR4_B 6 +#define MOTOR3_A 5 +#define MOTOR3_B 7 + +// Constants that the user passes in to the motor calls +#define FORWARD 1 +#define BACKWARD 2 +#define LEFT 3 +#define RIGHT 4 +#define STOP 5 + +#define FORWARD2 2 +#define BACKWARD2 1 + +#define BRAKE 3 +#define RELEASE 4 + +// Constants that the user passes in to the stepper calls +#define SINGLE 1 +#define DOUBLE 2 +#define INTERLEAVE 3 +#define MICROSTEP 4 + +/* +#define LATCH 4 +#define LATCH_DDR DDRB +#define LATCH_PORT PORTB + +#define CLK_PORT PORTD +#define CLK_DDR DDRD +#define CLK 4 + +#define ENABLE_PORT PORTD +#define ENABLE_DDR DDRD +#define ENABLE 7 + +#define SER 0 +#define SER_DDR DDRB +#define SER_PORT PORTB +*/ + +// Arduino pin names for interface to 74HCT595 latch +#define MOTORLATCH 12 +#define MOTORCLK 4 +#define MOTORENABLE 7 +#define MOTORDATA 8 + +class UCMotorController +{ + public: + UCMotorController(void); + void enable(void); + friend class UC_DCMotor; + void latch_tx(void); + uint8_t TimerInitalized; +}; + +class UC_DCMotor +{ + public: + UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); + void run(uint8_t); + void setSpeed(uint8_t); + + private: + uint8_t motornum, pwmfreq; +}; + +class UC_Stepper { + public: + UC_Stepper(uint16_t, uint8_t); + void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); + void setSpeed(uint16_t); + uint8_t onestep(uint8_t dir, uint8_t style); + void release(void); + uint16_t revsteps; // # steps per revolution + uint8_t steppernum; + uint32_t usperstep, steppingcounter; + private: + uint8_t currentstep; + +}; + +uint8_t getlatchstate(void); + +#endif diff --git a/vehicle_interface/main/main.ino b/vehicle_interface/main/main.ino index fad7faf7..3554be29 100644 --- a/vehicle_interface/main/main.ino +++ b/vehicle_interface/main/main.ino @@ -3,6 +3,7 @@ #include "UCMotor.h" #include #include +#include //You will need to install this library volatile Array commands = all_brake_command; @@ -26,6 +27,9 @@ void setup() pinMode(RC_SWA_CHANNEL_PIN, INPUT); pinMode(RC_SWB_CHANNEL_PIN, INPUT); + //Initialize I2C protocol for ultrasonic sensor + i2c_init(); + // Indicate setup done Serial.println("Setup complete"); } @@ -60,11 +64,80 @@ void sendCommandMini(UC_DCMotor * uc_motor, wheel_motor_command_t command) { void loop() { + int distance = read_the_sensor_example(); + if (distance > 25){ // Update wheel commands commands = fetch_rc_commands(); + } + else{ + commands = all_brake_command; + } // Send commands to wheels for (int i = wheel_indices::Start + 1; i < wheel_indices::End; i++ ) { sendCommandMini(minis[i], commands[i]); } } + +////////////////////////////////////////////////////////// +// Code Example: Read the sensor at the default address // +////////////////////////////////////////////////////////// +int read_the_sensor_example(){ + boolean error = 0; //Create a bit to check for catch errors as needed. + int distance; + + //Take a range reading at the default address of 224 + error = start_sensor(224); //Start the sensor and collect any error codes. + if (!error){ //If you had an error starting the sensor there is little point in reading it as you will get old data. + delay(100); + distance = read_sensor(224); //reading the sensor will return an integer value -- if this value is 0 there was an error + //Serial.print("Distance:");Serial.println(range);Serial.print("cm"); + Serial.println((String)"Distance:"+distance+" cm"); + return distance; + } +} + +/////////////////////////////////////////////////// +// Function: Start a range reading on the sensor // +/////////////////////////////////////////////////// +//Uses the I2C library to start a sensor at the given address +//Collects and reports an error bit where: 1 = there was an error or 0 = there was no error. +//INPUTS: byte bit8address = the address of the sensor that we want to command a range reading +//OUPUTS: bit errorlevel = reports if the function was successful in taking a range reading: 1 = the function +// had an error, 0 = the function was successful +boolean start_sensor(byte bit8address){ + boolean errorlevel = 0; + bit8address = bit8address & B11111110; //Do a bitwise 'and' operation to force the last bit to be zero -- we are writing to the address. + errorlevel = !i2c_start(bit8address) | errorlevel; //Run i2c_start(address) while doing so, collect any errors where 1 = there was an error. + errorlevel = !i2c_write(81) | errorlevel; //Send the 'take range reading' command. (notice how the library has error = 0 so I had to use "!" (not) to invert the error) + i2c_stop(); + return errorlevel; +} + + + +/////////////////////////////////////////////////////////////////////// +// Function: Read the range from the sensor at the specified address // +/////////////////////////////////////////////////////////////////////// +//Uses the I2C library to read a sensor at the given address +//Collects errors and reports an invalid range of "0" if there was a problem. +//INPUTS: byte bit8address = the address of the sensor to read from +//OUPUTS: int range = the distance in cm that the sensor reported; if "0" there was a communication error +int read_sensor(byte bit8address){ + boolean errorlevel = 0; + int range = 0; + byte range_highbyte = 0; + byte range_lowbyte = 0; + bit8address = bit8address | B00000001; //Do a bitwise 'or' operation to force the last bit to be 'one' -- we are reading from the address. + errorlevel = !i2c_start(bit8address) | errorlevel; + range_highbyte = i2c_read(0); //Read a byte and send an ACK (acknowledge) + range_lowbyte = i2c_read(1); //Read a byte and send a NACK to terminate the transmission + i2c_stop(); + range = (range_highbyte * 256) + range_lowbyte; //compile the range integer from the two bytes received. + if(errorlevel){ + return 0; + } + else{ + return range; + } +} diff --git a/vehicle_interface/main/pinout.h b/vehicle_interface/main/pinout.h index f507f43a..15563540 100644 --- a/vehicle_interface/main/pinout.h +++ b/vehicle_interface/main/pinout.h @@ -4,6 +4,11 @@ // Pins defined by the UCMotor header #include "UCMotor.h" +//Pin definitions for the ultrasonic sensor(MB1202) +#define SCL_PIN 5 //Default SDA is Pin5 PORTC for the UNO -- you can set this to any tristate pin +#define SCL_PORT PORTC +#define SDA_PIN 4 //Default SCL is Pin4 PORTC for the UNO -- you can set this to any tristate pin +#define SDA_PORT PORTC // Pin definitions for the RC receiver #ifdef __AVR_ATmega328P__ @@ -22,4 +27,4 @@ #error "No RC pin configurations available" #endif -#endif // !PINOUT_H +#endif // !PINOUT_H diff --git a/vehicle_interface/main/rc_commands.cpp b/vehicle_interface/main/rc_commands.cpp index 61af19f2..7910f36c 100644 --- a/vehicle_interface/main/rc_commands.cpp +++ b/vehicle_interface/main/rc_commands.cpp @@ -1,67 +1,67 @@ -#include "rc_commands.h" -#include "util.h" - - -/** - * @brief fetches RC commands and returns motor commands - * - * @return returns array of motor commands - */ -Array fetch_rc_commands() -{ - // Get commands - uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); - uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); - uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); - uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); - - // define temp variables - float right_duty = 0.0; - bool right_dir = DIR_FW; // default direction - float left_duty = 0.0; - bool left_dir = DIR_FW; // default direction - bool autonomy_mode = false; - - /* Switches */ - // Check if stop asserted - if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { - return all_brake_command; // fix to also return mode - } - // Check if mode changed - if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { - autonomy_mode = true; - } - - /* Right side longitudinal wheel set */ - // Forward - if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { - // map the duty from 0 to 1 given the min and max threshold values - right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) - { - right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); - right_dir = DIR_BW; - } - /* Left side longitudinal wheel set */ - // Forward - if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { - left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); - } - // Backward - else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) - { - left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); - left_dir = DIR_BW; - } - - /* Define and fill wheel motor commands */ - Array wheel_motor_commands = {{ - {left_duty, RELEASE_BRAKE, left_dir}, - {right_duty, RELEASE_BRAKE, right_dir}, - }}; - - /* Return reference to the command set */ - return wheel_motor_commands; -} +#include "rc_commands.h" +#include "util.h" + + +/** + * @brief fetches RC commands and returns motor commands + * + * @return returns array of motor commands + */ +Array fetch_rc_commands() +{ + // Get commands + uint16_t sw_a = pulseIn(RC_SWA_CHANNEL_PIN, HIGH); + uint16_t sw_b = pulseIn(RC_SWB_CHANNEL_PIN, HIGH); + uint16_t rc_right = pulseIn(RC_RIGHT_CHANNEL_PIN, HIGH); + uint16_t rc_left = pulseIn(RC_LEFT_CHANNEL_PIN, HIGH); + + // define temp variables + float right_duty = 0.0; + bool right_dir = DIR_FW; // default direction + float left_duty = 0.0; + bool left_dir = DIR_FW; // default direction + bool autonomy_mode = false; + + /* Switches */ + // Check if stop asserted + if (sw_a < RC_SWX_HIGH_MAX && sw_a > RC_SWX_HIGH_MIN) { + return all_brake_command; // fix to also return mode + } + // Check if mode changed + if (sw_b < RC_SWX_HIGH_MAX && sw_b > RC_SWX_HIGH_MIN) { + autonomy_mode = true; + } + + /* Right side longitudinal wheel set */ + // Forward + if (rc_right < RC_RIGHT_SET_FW_MAX && rc_right > RC_RIGHT_SET_FW_MIN) { + // map the duty from 0 to 1 given the min and max threshold values + right_duty = linMapToFloat(rc_right, RC_RIGHT_SET_FW_MIN, RC_RIGHT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_right < RC_RIGHT_SET_BW_MAX && rc_right > RC_RIGHT_SET_BW_MIN) + { + right_duty = 1 - linMapToFloat(rc_right, RC_RIGHT_SET_BW_MIN, RC_RIGHT_SET_BW_MAX, 0, 1); + right_dir = DIR_BW; + } + /* Left side longitudinal wheel set */ + // Forward + if (rc_left < RC_LEFT_SET_FW_MAX && rc_left > RC_LEFT_SET_FW_MIN) { + left_duty = linMapToFloat(rc_left, RC_LEFT_SET_FW_MIN, RC_LEFT_SET_FW_MAX, 0, 1); + } + // Backward + else if (rc_left < RC_LEFT_SET_BW_MAX && rc_left > RC_LEFT_SET_BW_MIN) + { + left_duty = 1 - linMapToFloat(rc_left, RC_LEFT_SET_BW_MIN, RC_LEFT_SET_BW_MAX, 0, 1); + left_dir = DIR_BW; + } + + /* Define and fill wheel motor commands */ + Array wheel_motor_commands = {{ + {left_duty, RELEASE_BRAKE, left_dir}, + {right_duty, RELEASE_BRAKE, right_dir}, + }}; + + /* Return reference to the command set */ + return wheel_motor_commands; +} diff --git a/vehicle_interface/main/rc_commands.h b/vehicle_interface/main/rc_commands.h index c47125c0..713de962 100644 --- a/vehicle_interface/main/rc_commands.h +++ b/vehicle_interface/main/rc_commands.h @@ -1,28 +1,28 @@ -#ifndef RC_COMMANDS_H -#define RC_COMMANDS_H -#include "pinout.h" -#include "wheel.h" -#include "util.h" - -/* Define the max and min thresholds for the longitudinal wheel sets */ -// CH2 - right longitudinal wheel set -#define RC_RIGHT_SET_FW_MAX 1985 -#define RC_RIGHT_SET_FW_MIN 1638 -#define RC_RIGHT_SET_BW_MAX 1310 -#define RC_RIGHT_SET_BW_MIN 980 -// CH3 - left longitudinal wheel set -#define RC_LEFT_SET_FW_MAX 1972 -#define RC_LEFT_SET_FW_MIN 1632 -#define RC_LEFT_SET_BW_MAX 1320 -#define RC_LEFT_SET_BW_MIN 997//1007 -/* Define channels and thresholds for the switches */ -// CH5/6 - SWA/B -#define RC_SWX_LOW_MAX 1000//994 -#define RC_SWX_LOW_MIN 950//987 -#define RC_SWX_HIGH_MAX 2000//1981 -#define RC_SWX_HIGH_MIN 1900//1974 - -/* Function prototypes for rc commands */ -Array fetch_rc_commands(void); - +#ifndef RC_COMMANDS_H +#define RC_COMMANDS_H +#include "pinout.h" +#include "wheel.h" +#include "util.h" + +/* Define the max and min thresholds for the longitudinal wheel sets */ +// CH2 - right longitudinal wheel set +#define RC_RIGHT_SET_FW_MAX 1985 +#define RC_RIGHT_SET_FW_MIN 1638 +#define RC_RIGHT_SET_BW_MAX 1310 +#define RC_RIGHT_SET_BW_MIN 980 +// CH3 - left longitudinal wheel set +#define RC_LEFT_SET_FW_MAX 1972 +#define RC_LEFT_SET_FW_MIN 1632 +#define RC_LEFT_SET_BW_MAX 1320 +#define RC_LEFT_SET_BW_MIN 997//1007 +/* Define channels and thresholds for the switches */ +// CH5/6 - SWA/B +#define RC_SWX_LOW_MAX 1000//994 +#define RC_SWX_LOW_MIN 950//987 +#define RC_SWX_HIGH_MAX 2000//1981 +#define RC_SWX_HIGH_MIN 1900//1974 + +/* Function prototypes for rc commands */ +Array fetch_rc_commands(void); + #endif // !RC_COMMANDS_H \ No newline at end of file diff --git a/vehicle_interface/main/util.cpp b/vehicle_interface/main/util.cpp index 17dc0b7a..04af3169 100644 --- a/vehicle_interface/main/util.cpp +++ b/vehicle_interface/main/util.cpp @@ -1,14 +1,14 @@ -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} diff --git a/vehicle_interface/main/util.h b/vehicle_interface/main/util.h index 40a279e9..b62bc666 100644 --- a/vehicle_interface/main/util.h +++ b/vehicle_interface/main/util.h @@ -1,53 +1,53 @@ -#ifndef UTIL_H -#define UTIL_H - -#include - -#define LN_SCALE_FACTOR -3.0 -#define MAX_DUTY 0.95 - -/** - * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. - * - * @param x input value - * @param in_min input minimum - * @param in_max input maximum - * @param out_min output minimum - * @param out_max output maximum - * @return float - */ -float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); - -template -struct Array { - // Storage - T data[N]; - - static size_t length() { return N; } - using type = T; - - // Item access - T &operator[](size_t index) { return data[index]; } - const T &operator[](size_t index) const { return data[index]; } - - // Iterators - T *begin() { return &data[0]; } - const T *begin() const { return &data[0]; } - T *end() { return &data[N]; } - const T *end() const { return &data[N]; } - - // Comparisons - bool operator==(const Array &rhs) const { - if (this == &rhs) - return true; - for (size_t i = 0; i < N; i++) - if ((*this)[i] != rhs[i]) - return false; - return true; - } - bool operator!=(const Array &rhs) const { - return !(*this == rhs); - } -}; - -#endif // UTIL_H +#ifndef UTIL_H +#define UTIL_H + +#include + +#define LN_SCALE_FACTOR -3.0 +#define MAX_DUTY 0.95 + +/** + * @brief Returns the input linearly mapped to [0, 1] based on the provided I/O minima and maxima. + * + * @param x input value + * @param in_min input minimum + * @param in_max input maximum + * @param out_min output minimum + * @param out_max output maximum + * @return float + */ +float linMapToFloat(float x, float in_min, float in_max, float out_min, float out_max); + +template +struct Array { + // Storage + T data[N]; + + static size_t length() { return N; } + using type = T; + + // Item access + T &operator[](size_t index) { return data[index]; } + const T &operator[](size_t index) const { return data[index]; } + + // Iterators + T *begin() { return &data[0]; } + const T *begin() const { return &data[0]; } + T *end() { return &data[N]; } + const T *end() const { return &data[N]; } + + // Comparisons + bool operator==(const Array &rhs) const { + if (this == &rhs) + return true; + for (size_t i = 0; i < N; i++) + if ((*this)[i] != rhs[i]) + return false; + return true; + } + bool operator!=(const Array &rhs) const { + return !(*this == rhs); + } +}; + +#endif // UTIL_H diff --git a/vehicle_interface/main/wheel.h b/vehicle_interface/main/wheel.h index dce2afb0..e7db122e 100644 --- a/vehicle_interface/main/wheel.h +++ b/vehicle_interface/main/wheel.h @@ -9,7 +9,7 @@ /* Define Important Values */ #if defined(__AVR_ATmega328P__) #define TOP 0XFF - #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #elif (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)) #define TOP 0x03FF // Top value of the registers (configuration done in MAIN) #endif @@ -48,4 +48,4 @@ const Array all_brake_command = {{ {0.0, ENGAGE_BRAKE, FORWARD} }}; -#endif // !Wheel_h +#endif // !Wheel_h From 78bcc70f04d842fd1ac8469fc09f066667cb9e72 Mon Sep 17 00:00:00 2001 From: zhaoshengEE Date: Sun, 29 Mar 2020 15:18:13 -0700 Subject: [PATCH 5/6] RC_car_with_US_V2.0 --- vehicle_interface/main/README.md | 22 +- vehicle_interface/main/SoftI2CMaster.h | 862 ------------------------- vehicle_interface/main/UCMotor.cpp | 729 --------------------- vehicle_interface/main/UCMotor.h | 195 ------ vehicle_interface/main/main.ino | 8 +- vehicle_interface/main/rc_commands.h | 58 +- 6 files changed, 45 insertions(+), 1829 deletions(-) delete mode 100644 vehicle_interface/main/SoftI2CMaster.h delete mode 100644 vehicle_interface/main/UCMotor.cpp delete mode 100644 vehicle_interface/main/UCMotor.h diff --git a/vehicle_interface/main/README.md b/vehicle_interface/main/README.md index 711e98df..523d244b 100644 --- a/vehicle_interface/main/README.md +++ b/vehicle_interface/main/README.md @@ -1,11 +1,11 @@ -# PropBot - -An I2C library is necessary for this project. - -Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster - -And install the library onto Arduino IDE. - -For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries - -Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car +# PropBot + +An I2C library is necessary for this project. + +Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster + +And install the library onto Arduino IDE. + +For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries + +Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car \ No newline at end of file diff --git a/vehicle_interface/main/SoftI2CMaster.h b/vehicle_interface/main/SoftI2CMaster.h deleted file mode 100644 index 68de1a9a..00000000 --- a/vehicle_interface/main/SoftI2CMaster.h +++ /dev/null @@ -1,862 +0,0 @@ -/* Arduino SoftI2C library. - * - * Version 1.4 - * - * Copyright (C) 2013, Bernhard Nebel and Peter Fleury - * - * This is a very fast and very light-weight software I2C-master library - * written in assembler. It is based on Peter Fleury's I2C software - * library: http://homepage.hispeed.ch/peterfleury/avr-software.html - * Recently, the hardware implementation has been added to the code, - * which can be enabled by defining I2C_HARDWARE. - * - * This Library is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This Library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with the Arduino I2cMaster Library. If not, see - * . - */ - -/* In order to use the library, you need to define SDA_PIN, SCL_PIN, - * SDA_PORT and SCL_PORT before including this file. Have a look at - * http://www.arduino.cc/en/Reference/PortManipulation for finding out - * which values to use. For example, if you use digital pin 3 (corresponding - * to PD3) for SDA and digital pin 13 (corresponding to PB5) - * for SCL on a standard Arduino, - * you have to use the following definitions: - * #define SDA_PIN 3 - * #define SDA_PORT PORTD - * #define SCL_PIN 5 - * #define SCL_PORT PORTB - * - * Alternatively, you can define the compile time constant I2C_HARDWARE, - * in which case the TWI hardware is used. In this case you have to use - * the standard SDA/SCL pins (and, of course, the chip needs to support - * this). - * - * You can also define the following constants (see also below): - ' - I2C_PULLUP = 1 meaning that internal pullups should be used - * - I2C_CPUFREQ, when changing CPU clock frequency dynamically - * - I2C_FASTMODE = 1 meaning that the I2C bus allows speeds up to 400 kHz - * - I2C_SLOWMODE = 1 meaning that the I2C bus will allow only up to 25 kHz - * - I2C_NOINTERRUPT = 1 in order to prohibit interrupts while - * communicating (see below). This can be useful if you use the library - * for communicationg with SMbus devices, which have timeouts. - * Note, however, that interrupts are disabled from issuing a start condition - * until issuing a stop condition. So use this option with care! - * - I2C_TIMEOUT = 0..10000 msec in order to return from the I2C functions - * in case of a I2C bus lockup (i.e., SCL constantly low). 0 means no timeout. - * - I2C_MAXWAIT = 0..32767 number of retries in i2c_start_wait. 0 means never stop. - */ - -/* Changelog: - * Version 2.1 - * - added conditional to check whether it is the right MCU type - * Version 2.0 - * - added hardware support as well. - * Version 1.4: - * - added "maximum retry" in i2c_start_wait in order to avoid lockup - * - added "internal pullups", but be careful since the option stretches the I2C specs - * Version 1.3: - * - added "__attribute__ ((used))" for all functions declared with "__attribute__ ((noinline))" - * Now the module is also usable in Arduino 1.6.11+ - * Version 1.2: - * - added pragma to avoid "unused parameter warnings" (suggestion by Walter) - * - replaced wrong license file - * Version 1.1: - * - removed I2C_CLOCK_STRETCHING - * - added I2C_TIMEOUT time in msec (0..10000) until timeout or 0 if no timeout - * - changed i2c_init to return true iff both SDA and SCL are high - * - changed interrupt disabling so that the previous IRQ state is restored - * Version 1.0: basic functionality - */ - -#ifndef __AVR_ARCH__ -#error "Not an AVR MCU! Use 'SlowSoftI2CMaster' library instead of 'SoftI2CMaster'!" -#else - -#ifndef _SOFTI2C_H -#define _SOFTI2C_H 1 - -#include -#include -#include - -#pragma GCC diagnostic push - -#pragma GCC diagnostic ignored "-Wunused-parameter" - - - -// Init function. Needs to be called once in the beginning. -// Returns false if SDA or SCL are low, which probably means -// a I2C bus lockup or that the lines are not pulled up. -bool __attribute__ ((noinline)) i2c_init(void) __attribute__ ((used)); - -// Start transfer function: is the 8-bit I2C address (including the R/W -// bit). -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_start(uint8_t addr) __attribute__ ((used)); - -// Similar to start function, but wait for an ACK! Will timeout if I2C_MAXWAIT > 0. -bool __attribute__ ((noinline)) i2c_start_wait(uint8_t addr) __attribute__ ((used)); - -// Repeated start function: After having claimed the bus with a start condition, -// you can address another or the same chip again without an intervening -// stop condition. -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_rep_start(uint8_t addr) __attribute__ ((used)); - -// Issue a stop condition, freeing the bus. -void __attribute__ ((noinline)) i2c_stop(void) asm("ass_i2c_stop") __attribute__ ((used)); - -// Write one byte to the slave chip that had been addressed -// by the previous start call. is the byte to be sent. -// Return: true if the slave replies with an "acknowledge", false otherwise -bool __attribute__ ((noinline)) i2c_write(uint8_t value) asm("ass_i2c_write") __attribute__ ((used)); - - -// Read one byte. If is true, we send a NAK after having received -// the byte in order to terminate the read sequence. -uint8_t __attribute__ ((noinline)) i2c_read(bool last) __attribute__ ((used)); - -// If you want to use the TWI hardeware, you have to define I2C_HARDWARE to be 1 -#ifndef I2C_HARDWARE -#define I2C_HARDWARE 0 -#endif - -#if I2C_HARDWARE -#ifndef TWDR -#error This chip does not support hardware I2C. Please undfine I2C_HARDWARE -#endif -#endif - -// You can set I2C_CPUFREQ independently of F_CPU if you -// change the CPU frequency on the fly. If you do not define it, -// it will use the value of F_CPU -#ifndef I2C_CPUFREQ -#define I2C_CPUFREQ F_CPU -#endif - -// If I2C_FASTMODE is set to 1, then the highest possible frequency below 400kHz -// is selected. Be aware that not all slave chips may be able to deal with that! -#ifndef I2C_FASTMODE -#define I2C_FASTMODE 0 -#endif - -// If I2C_FASTMODE is not defined or defined to be 0, then you can set -// I2C_SLOWMODE to 1. In this case, the I2C frequency will not be higher -// than 25KHz. This could be useful for problematic buses with high pull-ups -// and high capasitance. -#ifndef I2C_SLOWMODE -#define I2C_SLOWMODE 0 -#endif - -// If I2C_PULLUP is set to 1, then the internal pull-up resistors are used. -// This does not conform with the I2C specs, since the bus lines will be -// temporarily in high-state and the internal resistors have roughly 50k. -// With low bus speeds und short buses it usually works, though (hopefully). -#ifndef I2C_PULLUP -#define I2C_PULLUP 0 -#endif - -// if I2C_NOINTERRUPT is 1, then the I2C routines are not interruptable. -// This is most probably only necessary if you are using a 1MHz system clock, -// you are communicating with a SMBus device, and you want to avoid timeouts. -// Be aware that the interrupt bit is enabled after each call. So the -// I2C functions should not be called in interrupt routines or critical regions. -#ifndef I2C_NOINTERRUPT -#define I2C_NOINTERRUPT 0 -#endif - -// I2C_TIMEOUT can be set to a value between 1 and 10000. -// If it is defined and nonzero, it leads to a timeout if the -// SCL is low longer than I2C_TIMEOUT milliseconds, i.e., max timeout is 10 sec -#ifndef I2C_TIMEOUT -#define I2C_TIMEOUT 0 -#else -#if I2C_TIMEOUT > 10000 -#error I2C_TIMEOUT is too large -#endif -#endif - -// I2C_MAXWAIT can be set to any value between 0 and 32767. 0 means no time out. -#ifndef I2C_MAXWAIT -#define I2C_MAXWAIT 500 -#else -#if I2C_MAXWAIT > 32767 || I2C_MAXWAIT < 0 -#error Illegal I2C_MAXWAIT value -#endif -#endif - -#define I2C_TIMEOUT_DELAY_LOOPS (I2C_CPUFREQ/1000UL)*I2C_TIMEOUT/4000UL -#if I2C_TIMEOUT_DELAY_LOOPS < 1 -#define I2C_MAX_STRETCH 1 -#else -#if I2C_TIMEOUT_DELAY_LOOPS > 60000UL -#define I2C_MAX_STRETCH 60000UL -#else -#define I2C_MAX_STRETCH I2C_TIMEOUT_DELAY_LOOPS -#endif -#endif - -#if I2C_FASTMODE -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/350000L)/2-18)/3) -#define SCL_CLOCK 400000UL -#else -#if I2C_SLOWMODE -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/23500L)/2-18)/3) -#define SCL_CLOCK 25000UL -#else -#define I2C_DELAY_COUNTER (((I2C_CPUFREQ/90000L)/2-18)/3) -#define SCL_CLOCK 100000UL -#endif -#endif - -// constants for reading & writing -#define I2C_READ 1 -#define I2C_WRITE 0 - -#if !I2C_HARDWARE -// map the IO register back into the IO address space -#define SDA_DDR (_SFR_IO_ADDR(SDA_PORT) - 1) -#define SCL_DDR (_SFR_IO_ADDR(SCL_PORT) - 1) -#define SDA_OUT _SFR_IO_ADDR(SDA_PORT) -#define SCL_OUT _SFR_IO_ADDR(SCL_PORT) -#define SDA_IN (_SFR_IO_ADDR(SDA_PORT) - 2) -#define SCL_IN (_SFR_IO_ADDR(SCL_PORT) - 2) - -#ifndef __tmp_reg__ -#define __tmp_reg__ 0 -#endif - -// Internal delay functions. -void __attribute__ ((noinline)) i2c_delay_half(void) asm("ass_i2c_delay_half") __attribute__ ((used)); -void __attribute__ ((noinline)) i2c_wait_scl_high(void) asm("ass_i2c_wait_scl_high") __attribute__ ((used)); - -void i2c_delay_half(void) -{ // function call 3 cycles => 3C -#if I2C_DELAY_COUNTER < 1 - __asm__ __volatile__ (" ret"); - // 7 cycles for call and return -#else - __asm__ __volatile__ - ( - " ldi r25, %[DELAY] ;load delay constant ;; 4C \n\t" - "_Lidelay: \n\t" - " dec r25 ;decrement counter ;; 4C+xC \n\t" - " brne _Lidelay ;;5C+(x-1)2C+xC\n\t" - " ret ;; 9C+(x-1)2C+xC = 7C+xC" - : : [DELAY] "M" I2C_DELAY_COUNTER : "r25"); - // 7 cycles + 3 times x cycles -#endif -} - -void i2c_wait_scl_high(void) -{ -#if I2C_TIMEOUT <= 0 - __asm__ __volatile__ - ("_Li2c_wait_stretch: \n\t" - " sbis %[SCLIN],%[SCLPIN] ;wait for SCL high \n\t" - " rjmp _Li2c_wait_stretch \n\t" - " cln ;signal: no timeout \n\t" - " ret " - : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN)); -#else - __asm__ __volatile__ - ( " ldi r27, %[HISTRETCH] ;load delay counter \n\t" - " ldi r26, %[LOSTRETCH] \n\t" - "_Lwait_stretch: \n\t" - " clr __tmp_reg__ ;do next loop 255 times \n\t" - "_Lwait_stretch_inner_loop: \n\t" - " rcall _Lcheck_scl_level ;call check function ;; 12C \n\t" - " brpl _Lstretch_done ;done if N=0 ;; +1 = 13C\n\t" - " dec __tmp_reg__ ;dec inner loop counter;; +1 = 14C\n\t" - " brne _Lwait_stretch_inner_loop ;; +2 = 16C\n\t" - " sbiw r26,1 ;dec outer loop counter \n\t" - " brne _Lwait_stretch ;continue with outer loop \n\t" - " sen ;timeout -> set N-bit=1 \n\t" - " rjmp _Lwait_return ;and return with N=1\n\t" - "_Lstretch_done: ;SCL=1 sensed \n\t" - " cln ;OK -> clear N-bit \n\t" - " rjmp _Lwait_return ; and return with N=0 \n\t" - - "_Lcheck_scl_level: ;; call = 3C\n\t" - " cln ;; +1C = 4C \n\t" - " sbic %[SCLIN],%[SCLPIN] ;skip if SCL still low ;; +2C = 6C \n\t" - " rjmp _Lscl_high ;; +0C = 6C \n\t" - " sen ;; +1 = 7C\n\t " - "_Lscl_high: " - " nop ;; +1C = 8C \n\t" - " ret ;return N-Bit=1 if low ;; +4 = 12C\n\t" - - "_Lwait_return:" - : : [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), - [HISTRETCH] "M" (I2C_MAX_STRETCH>>8), - [LOSTRETCH] "M" (I2C_MAX_STRETCH&0xFF) - : "r26", "r27"); -#endif -} -#endif // !I2C_HARDWARE - -bool i2c_init(void) -#if I2C_HARDWARE -{ -#if I2C_PULLUP - digitalWrite(SDA, 1); - digitalWrite(SCL, 1); -#else - digitalWrite(SDA, 0); - digitalWrite(SCL, 0); -#endif -#if ((I2C_CPUFREQ/SCL_CLOCK)-16)/2 < 250 - TWSR = 0; /* no prescaler */ - TWBR = ((I2C_CPUFREQ/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ -#else - TWSR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_START) && (twst != TW_REP_START)) return false; - - // send device address - TWDR = addr; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return false; - - return true; -} -#else -{ - __asm__ __volatile__ - ( -#if I2C_NOINTERRUPT - " cli ;clear IRQ bit \n\t" -#endif - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;wait T/2 \n\t" - " rcall ass_i2c_write ;now write address \n\t" - " ret" - : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SCLIN] "I" (SCL_IN),[SCLPIN] "I" (SCL_PIN)); - return true; // we never return here! -} -#endif - -bool i2c_rep_start(uint8_t addr) -#if I2C_HARDWARE -{ - return i2c_start(addr); -} -#else -{ - __asm__ __volatile__ - - ( -#if I2C_NOINTERRUPT - " cli \n\t" -#endif -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable SDA pull-up\n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " rcall ass_i2c_write \n\t" - " ret" - : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), - [SCLIN] "I" (SCL_IN), [SCLOUT] "I" (SCL_OUT), [SDAOUT] "I" (SDA_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); - return true; // just to fool the compiler -} -#endif - -bool i2c_start_wait(uint8_t addr) -#if I2C_HARDWARE -{ - uint8_t twst; - uint16_t maxwait = I2C_MAXWAIT; -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - while (true) { - // send START condition - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst != TW_START) && (twst != TW_REP_START)) continue; - - // send device address - TWDR = addr; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits. - twst = TW_STATUS & 0xF8; - if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) - { - /* device busy, send stop condition to terminate write operation */ - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - if (maxwait) - if (--maxwait == 0) - return false; - - continue; - } - //if( twst != TW_MT_SLA_ACK) return 1; - return true; - } -} -#else -{ - __asm__ __volatile__ - ( - " push r24 ;save original parameter \n\t" -#if I2C_MAXWAIT - " ldi r31, %[HIMAXWAIT] ;load max wait counter \n\t" - " ldi r30, %[LOMAXWAIT] ;load low byte \n\t" -#endif - "_Li2c_start_wait1: \n\t" - " pop r24 ;restore original parameter\n\t" - " push r24 ;and save again \n\t" -#if I2C_NOINTERRUPT - " cli ;disable interrupts \n\t" -#endif - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;delay T/2 \n\t" - " rcall ass_i2c_write ;write address \n\t" - " tst r24 ;if device not busy -> done \n\t" - " brne _Li2c_start_wait_done \n\t" - " rcall ass_i2c_stop ;terminate write & enable IRQ \n\t" -#if I2C_MAXWAIT - " sbiw r30,1 ;decrement max wait counter\n\t" - " breq _Li2c_start_wait_done ;if zero reached, exit with false -> r24 already zero!\n\t" -#endif - " rjmp _Li2c_start_wait1 ;device busy, poll ack again \n\t" - "_Li2c_start_wait_done: \n\t" - " clr r25 ;clear high byte of return value\n\t" - " pop __tmp_reg__ ;pop off orig argument \n\t" - " ret " - : : [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAOUT] "I" (SDA_OUT), - [SCLIN] "I" (SCL_IN), [SCLPIN] "I" (SCL_PIN), - [HIMAXWAIT] "M" (I2C_MAXWAIT>>8), - [LOMAXWAIT] "M" (I2C_MAXWAIT&0xFF) - : "r30", "r31" ); -} -#endif - -void i2c_stop(void) -#if I2C_HARDWARE -{ -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - /* send stop condition */ - TWCR = (1< I2C_TIMEOUT) return; -#endif - } -} -#else -{ - __asm__ __volatile__ - ( -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low \n\t" - " rcall ass_i2c_delay_half ;T/2 delay \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " rcall ass_i2c_delay_half ;T/2 delay \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for clock stretching slave\n\t" - " rcall ass_i2c_wait_scl_high ;wait until SCL=H\n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rcall ass_i2c_delay_half \n\t" -#if I2C_NOINTERRUPT - " sei ;enable interrupts again!\n\t" -#endif - : : [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN)); -} -#endif - - -bool i2c_write(uint8_t value) -#if I2C_HARDWARE -{ - uint8_t twst; -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - - // send data to the previously addressed device - TWDR = value; - TWCR = (1< I2C_TIMEOUT) return false; -#endif - } - - // check value of TWI Status Register. Mask prescaler bits - twst = TW_STATUS & 0xF8; - if( twst != TW_MT_DATA_ACK) return false; - return true; -} -#else -{ - __asm__ __volatile__ - ( - " sec ;set carry flag \n\t" - " rol r24 ;shift in carry and shift out MSB \n\t" - " rjmp _Li2c_write_first \n\t" - "_Li2c_write_bit:\n\t" - " lsl r24 ;left shift into carry ;; 1C\n\t" - "_Li2c_write_first:\n\t" - " breq _Li2c_get_ack ;jump if TXreg is empty;; +1 = 2C \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 4C \n\t" - " nop \n\t" - " nop \n\t" - " nop \n\t" - " brcc _Li2c_write_low ;;+1/+2=5/6C\n\t" - " nop ;; +1 = 7C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA ;; +2 = 9C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rjmp _Li2c_write_high ;; +2 = 11C \n\t" - "_Li2c_write_low: \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; +2 = 9C \n\t" - " rjmp _Li2c_write_high ;;+2 = 11C \n\t" - "_Li2c_write_high: \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;;+X = 11C+X\n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;;+2 = 13C+X\n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " cln ;clear N-bit ;;+1 = 14C+X\n\t" - " nop \n\t" - " nop \n\t" - " nop \n\t" - " sbis %[SCLIN],%[SCLPIN] ;check for SCL high ;;+2 = 16C+X\n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brpl _Ldelay_scl_high ;;+2 = 18C+X\n\t" - "_Li2c_write_return_false: \n\t" - " clr r24 ; return false because of timeout \n\t" - " rjmp _Li2c_write_return \n\t" - "_Ldelay_scl_high: \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;;+X= 18C+2X\n\t" -#endif - " rjmp _Li2c_write_bit \n\t" - " ;; +2 = 20C +2X for one bit-loop \n\t" - "_Li2c_get_ack: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; +2 = 5C \n\t" - " nop \n\t" - " nop \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA ;;+2 = 7C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; +X = 7C+X \n\t" -#endif - " clr r25 ;; 17C+2X \n\t" - " clr r24 ;return 0 ;; 14C + X \n\t" - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; +2 = 9C+X\n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - "_Li2c_ack_wait: \n\t" - " cln ; clear N-bit ;; 10C + X\n\t" - " nop \n\t" - " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X \n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brmi _Li2c_write_return_false ;; 13C + X \n\t " - " sbis %[SDAIN],%[SDAPIN] ;if SDA hi -> return 0 ;; 15C + X \n\t" - " ldi r24,1 ;return true ;; 16C + X \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 16C + 2X \n\t" -#endif - "_Li2c_write_return: \n\t" - " nop \n\t " - " nop \n\t " -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" - " ret \n\t" - " ;; + 4 = 17C + 2X for acknowldge bit" - :: - [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN)); - return true; // fooling the compiler -} -#endif - -uint8_t i2c_read(bool last) -#if I2C_HARDWARE -{ -#if I2C_TIMEOUT - uint32_t start = millis(); -#endif - - TWCR = (1< I2C_TIMEOUT) return 0xFF; -#endif - } - return TWDR; -} -#else -{ - __asm__ __volatile__ - ( - " ldi r23,0x01 \n\t" - "_Li2c_read_bit: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA(prev. ACK);; 4C \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " nop \n\t" - " nop \n\t" - " nop \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 4C+X \n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 6C + X \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 6C + 2X \n\t" -#endif - " cln ; clear N-bit ;; 7C + 2X \n\t" - " nop \n\t " - " nop \n\t " - " nop \n\t " - " sbis %[SCLIN], %[SCLPIN] ;check for SCL high ;; 9C +2X \n\t" - " rcall ass_i2c_wait_scl_high \n\t" - " brmi _Li2c_read_return ;return if timeout ;; 10C + 2X\n\t" - " clc ;clear carry flag ;; 11C + 2X\n\t" - " sbic %[SDAIN],%[SDAPIN] ;if SDA is high ;; 11C + 2X\n\t" - " sec ;set carry flag ;; 12C + 2X\n\t" - " rol r23 ;store bit ;; 13C + 2X\n\t" - " brcc _Li2c_read_bit ;while receiv reg not full \n\t" - " ;; 15C + 2X for one bit loop \n\t" - - "_Li2c_put_ack: \n\t" -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - " sbi %[SCLDDR],%[SCLPIN] ;force SCL low ;; 2C \n\t" - " cpi r24,0 ;; 3C \n\t" - " breq _Li2c_put_ack_low ;if (ack=0) ;; 5C \n\t" - " cbi %[SDADDR],%[SDAPIN] ;release SDA \n\t" -#if I2C_PULLUP - " sbi %[SDAOUT],%[SDAPIN] ;enable SDA pull-up \n\t" -#endif - " rjmp _Li2c_put_ack_high \n\t" - "_Li2c_put_ack_low: ;else \n\t" -#if I2C_PULLUP - " cbi %[SDAOUT],%[SDAPIN] ;disable pull-up \n\t" -#endif - " sbi %[SDADDR],%[SDAPIN] ;force SDA low ;; 7C \n\t" - "_Li2c_put_ack_high: \n\t" - " nop \n\t " - " nop \n\t " - " nop \n\t " -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 7C + X \n\t" -#endif - " cbi %[SCLDDR],%[SCLPIN] ;release SCL ;; 9C +X \n\t" -#if I2C_PULLUP - " sbi %[SCLOUT],%[SCLPIN] ;enable SCL pull-up \n\t" -#endif - " cln ;clear N ;; +1 = 10C\n\t" - " nop \n\t " - " nop \n\t " - " sbis %[SCLIN],%[SCLPIN] ;wait SCL high ;; 12C + X\n\t" - " rcall ass_i2c_wait_scl_high \n\t" -#if I2C_DELAY_COUNTER >= 1 - " rcall ass_i2c_delay_half ;delay T/2 ;; 11C + 2X\n\t" -#endif - "_Li2c_read_return: \n\t" - " nop \n\t " - " nop \n\t " -#if I2C_PULLUP - " cbi %[SCLOUT],%[SCLPIN] ;disable SCL pull-up \n\t" -#endif - "sbi %[SCLDDR],%[SCLPIN] ;force SCL low so SCL=H is short\n\t" - " mov r24,r23 ;; 12C + 2X \n\t" - " clr r25 ;; 13 C + 2X\n\t" - " ret ;; 17C + X" - :: - [SCLDDR] "I" (SCL_DDR), [SCLPIN] "I" (SCL_PIN), [SCLIN] "I" (SCL_IN), - [SDAOUT] "I" (SDA_OUT), [SCLOUT] "I" (SCL_OUT), - [SDADDR] "I" (SDA_DDR), [SDAPIN] "I" (SDA_PIN), [SDAIN] "I" (SDA_IN) - ); - return ' '; // fool the compiler! -} -#endif - -#pragma GCC diagnostic pop - -#endif -#endif diff --git a/vehicle_interface/main/UCMotor.cpp b/vehicle_interface/main/UCMotor.cpp deleted file mode 100644 index 3978fc0e..00000000 --- a/vehicle_interface/main/UCMotor.cpp +++ /dev/null @@ -1,729 +0,0 @@ -/** - * @file UCMotor.cpp - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ - -// UCTRONOICS Motor shield library -// this code is public domain, enjoy! - - -#if (ARDUINO >= 100) - #include "Arduino.h" -#else - #if defined(__AVR__) - #include - #endif - #include "WProgram.h" -#endif - -#include "UCMotor.h" - - - -static uint8_t latch_state; - -#if (MICROSTEPS == 8) -uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255}; -#elif (MICROSTEPS == 16) -uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255}; -#endif - -UCMotorController::UCMotorController(void) { - TimerInitalized = false; -} - -void UCMotorController::enable(void) { - // setup the latch - /* - LATCH_DDR |= _BV(LATCH); - ENABLE_DDR |= _BV(ENABLE); - CLK_DDR |= _BV(CLK); - SER_DDR |= _BV(SER); - */ - pinMode(MOTORLATCH, OUTPUT); - pinMode(MOTORENABLE, OUTPUT); - pinMode(MOTORDATA, OUTPUT); - pinMode(MOTORCLK, OUTPUT); - - latch_state = 0; - - latch_tx(); // "reset" - - //ENABLE_PORT &= ~_BV(ENABLE); // enable the chip outputs! - digitalWrite(MOTORENABLE, LOW); -} - - -void UCMotorController::latch_tx(void) { - uint8_t i; - - //LATCH_PORT &= ~_BV(LATCH); - digitalWrite(MOTORLATCH, LOW); - - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - - for (i=0; i<8; i++) { - //CLK_PORT &= ~_BV(CLK); - digitalWrite(MOTORCLK, LOW); - - if (latch_state & _BV(7-i)) { - //SER_PORT |= _BV(SER); - digitalWrite(MOTORDATA, HIGH); - } else { - //SER_PORT &= ~_BV(SER); - digitalWrite(MOTORDATA, LOW); - } - //CLK_PORT |= _BV(CLK); - digitalWrite(MOTORCLK, HIGH); - } - //LATCH_PORT |= _BV(LATCH); - digitalWrite(MOTORLATCH, HIGH); -} - -static UCMotorController MC; - -/****************************************** - MOTORS -******************************************/ -inline void initPWM1(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a - TCCR2B = freq & 0x7; - OCR2A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - TCCR1A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc1a - TCCR1B = (freq & 0x7) | _BV(WGM12); - OCR1A = 0; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(9, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC4 (pin 9) in PWM mode, with Timer2 as timebase - OC4CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC4RS = 0x0000; - OC4R = 0x0000; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Make sure that pin 11 is an input, since we have tied together 9 and 11 - pinMode(10, OUTPUT); - pinMode(11, INPUT); - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC5 (pin 10) in PWM mode, with Timer2 as timebase - OC5CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC5RS = 0x0000; - OC5R = 0x0000; - #else - // If we are not using PWM for pin 11, then just do digital - digitalWrite(11, LOW); - #endif -#else - #error "This chip is not supported!" -#endif - #if !defined(PIC32_USE_PIN9_FOR_M1_PWM) && !defined(PIC32_USE_PIN10_FOR_M1_PWM) - pinMode(11, OUTPUT); - //Serial .println("OK"); - #endif -} - -inline void setPWM1(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR1A = s; -#elif defined(__PIC32MX__) - #if defined(PIC32_USE_PIN9_FOR_M1_PWM) - // Set the OC4 (pin 9) PMW duty cycle from 0 to 255 - OC4RS = s; - #elif defined(PIC32_USE_PIN10_FOR_M1_PWM) - // Set the OC5 (pin 10) PMW duty cycle from 0 to 255 - OC5RS = s; - #else - // If we are not doing PWM output for M1, then just use on/off - if (s > 127) - { - digitalWrite(11, HIGH); - } - else - { - digitalWrite(11, LOW); - } - #endif -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM2(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2B (pin 3) - TCCR2A |= _BV(COM2B1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2b - TCCR2B = freq & 0x7; - OCR2B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 3 is now PE5 (OC3C) - TCCR3A |= _BV(COM1C1) | _BV(WGM10); // fast PWM, turn on oc3c - TCCR3B = (freq & 0x7) | _BV(WGM12); - OCR3C = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC1 (pin3) in PWM mode, with Timer2 as timebase - OC1CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC1RS = 0x0000; - OC1R = 0x0000; -#else - #error "This chip is not supported!" -#endif - - pinMode(3, OUTPUT); -} - -inline void setPWM2(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer2A on PB3 (Arduino pin #11) - OCR2B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 11 is now PB5 (OC1A) - OCR3C = s; -#elif defined(__PIC32MX__) - // Set the OC1 (pin3) PMW duty cycle from 0 to 255 - OC1RS = s; -#else - #error "This chip is not supported!" -#endif -} - -inline void initPWM3(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A / PD6 (pin 6) - TCCR0A |= _BV(COM0A1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on OC0A - //TCCR0B = freq & 0x7; - OCR0A = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - TCCR4A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc4a - TCCR4B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR4A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC3 (pin 6) in PWM mode, with Timer2 as timebase - OC3CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC3RS = 0x0000; - OC3R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(6, OUTPUT); -} - -inline void setPWM3(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0A = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR4A = s; -#elif defined(__PIC32MX__) - // Set the OC3 (pin 6) PMW duty cycle from 0 to 255 - OC3RS = s; -#else - #error "This chip is not supported!" -#endif -} - - - -inline void initPWM4(uint8_t freq) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0B / PD5 (pin 5) - TCCR0A |= _BV(COM0B1) | _BV(WGM00) | _BV(WGM01); // fast PWM, turn on oc0a - //TCCR0B = freq & 0x7; - OCR0B = 0; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 5 is now PE3 (OC3A) - TCCR3A |= _BV(COM1A1) | _BV(WGM10); // fast PWM, turn on oc3a - TCCR3B = (freq & 0x7) | _BV(WGM12); - //TCCR4B = 1 | _BV(WGM12); - OCR3A = 0; -#elif defined(__PIC32MX__) - if (!MC.TimerInitalized) - { // Set up Timer2 for 80MHz counting fro 0 to 256 - T2CON = 0x8000 | ((freq & 0x07) << 4); // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=, T32=0, TCS=0; // ON=1, FRZ=0, SIDL=0, TGATE=0, TCKPS=0, T32=0, TCS=0 - TMR2 = 0x0000; - PR2 = 0x0100; - MC.TimerInitalized = true; - } - // Setup OC2 (pin 5) in PWM mode, with Timer2 as timebase - OC2CON = 0x8006; // OC32 = 0, OCTSEL=0, OCM=6 - OC2RS = 0x0000; - OC2R = 0x0000; -#else - #error "This chip is not supported!" -#endif - pinMode(5, OUTPUT); -} - -inline void setPWM4(uint8_t s) { -#if defined(__AVR_ATmega8__) || \ - defined(__AVR_ATmega48__) || \ - defined(__AVR_ATmega88__) || \ - defined(__AVR_ATmega168__) || \ - defined(__AVR_ATmega328P__) - // use PWM from timer0A on PB3 (Arduino pin #6) - OCR0B = s; -#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) - // on arduino mega, pin 6 is now PH3 (OC4A) - OCR3A = s; -#elif defined(__PIC32MX__) - // Set the OC2 (pin 5) PMW duty cycle from 0 to 255 - OC2RS = s; -#else - #error "This chip is not supported!" -#endif -} - -UC_DCMotor::UC_DCMotor(uint8_t num, uint8_t freq) { - motornum = num; - pwmfreq = freq; - MC.enable(); - - switch (num) { - case 1: - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM1(freq); - break; - case 2: - latch_state &= ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM2(freq); - break; - case 3: - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM3(freq); - break; - case 4: - latch_state &= ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // set both motor pins to 0 - MC.latch_tx(); - initPWM4(freq); - break; - } -} - -void UC_DCMotor::run(uint8_t cmd) { - uint8_t a, b; - switch (motornum) { - case 1: - a = MOTOR1_A; b = MOTOR1_B; break; - case 2: - a = MOTOR2_A; b = MOTOR2_B; break; - case 3: - a = MOTOR3_A; b = MOTOR3_B; break; - case 4: - a = MOTOR4_A; b = MOTOR4_B; break; - default: - return; - } - - switch (cmd) { - case FORWARD: - if(motornum ==3 || motornum ==4){ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - }else{ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - } - MC.latch_tx(); - break; - case BACKWARD: - if(motornum ==3 || motornum ==4){ - latch_state &= ~_BV(a); - latch_state |= _BV(b); - }else{ - latch_state |= _BV(a); - latch_state &= ~_BV(b); - } - MC.latch_tx(); - break; - case LEFT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - }else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx(); - } - break; - case RIGHT: - if(motornum ==3){ - a = MOTOR3_A; b = MOTOR3_B; //backward - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx();} - else if(motornum ==4){ - a = MOTOR4_A; b = MOTOR4_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==1){ - a = MOTOR1_A; b = MOTOR1_B; - latch_state &= ~_BV(a); - latch_state |= _BV(b); - MC.latch_tx();}else if(motornum ==2){ - a = MOTOR2_A; b = MOTOR2_B; - latch_state |= _BV(a); - latch_state &= ~_BV(b); - MC.latch_tx(); - } - break; - case STOP: - latch_state &= ~_BV(a); // A and B both low - latch_state &= ~_BV(b); - MC.latch_tx(); - break; - } -} - -void UC_DCMotor::setSpeed(uint8_t speed) { - switch (motornum) { - case 1: - setPWM1(speed); break; - case 2: - setPWM2(speed); break; - case 3: - setPWM3(speed); break; - case 4: - setPWM4(speed); break; - } -} - -/****************************************** - STEPPERS -******************************************/ - -UC_Stepper::UC_Stepper(uint16_t steps, uint8_t num) { - MC.enable(); - - revsteps = steps; - steppernum = num; - currentstep = 0; - - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(11, OUTPUT); - pinMode(3, OUTPUT); - digitalWrite(11, HIGH); - digitalWrite(3, HIGH); - - // use PWM for microstepping support - initPWM1(STEPPER1_PWM_RATE); - initPWM2(STEPPER1_PWM_RATE); - setPWM1(255); - setPWM2(255); - - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - - // enable both H bridges - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(5, HIGH); - digitalWrite(6, HIGH); - - // use PWM for microstepping support - // use PWM for microstepping support - initPWM3(STEPPER2_PWM_RATE); - initPWM4(STEPPER2_PWM_RATE); - setPWM3(255); - setPWM4(255); - } -} - -void UC_Stepper::setSpeed(uint16_t rpm) { - usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm); - steppingcounter = 0; -} - -void UC_Stepper::release(void) { - if (steppernum == 1) { - latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) & - ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0 - MC.latch_tx(); - } else if (steppernum == 2) { - latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) & - ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0 - MC.latch_tx(); - } -} - -void UC_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) { - uint32_t uspers = usperstep; - uint8_t ret = 0; - - if (style == INTERLEAVE) { - uspers /= 2; - } - else if (style == MICROSTEP) { - uspers /= MICROSTEPS; - steps *= MICROSTEPS; -#ifdef MOTORDEBUG - Serial.print("steps = "); Serial.println(steps, DEC); -#endif - } - - while (steps--) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - if (style == MICROSTEP) { - while ((ret != 0) && (ret != MICROSTEPS)) { - ret = onestep(dir, style); - delay(uspers/1000); // in ms - steppingcounter += (uspers % 1000); - if (steppingcounter >= 1000) { - delay(1); - steppingcounter -= 1000; - } - } - } -} - -uint8_t UC_Stepper::onestep(uint8_t dir, uint8_t style) { - uint8_t a, b, c, d; - uint8_t ocrb, ocra; - - ocra = ocrb = 255; - - if (steppernum == 1) { - a = _BV(MOTOR1_A); - b = _BV(MOTOR2_A); - c = _BV(MOTOR1_B); - d = _BV(MOTOR2_B); - } else if (steppernum == 2) { - a = _BV(MOTOR3_A); - b = _BV(MOTOR4_A); - c = _BV(MOTOR3_B); - d = _BV(MOTOR4_B); - } else { - return 0; - } - - // next determine what sort of stepping procedure we're up to - if (style == SINGLE) { - if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } - else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next even step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } - else { - currentstep -= MICROSTEPS; - } - } - } else if (style == DOUBLE) { - if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } else { // go to the next odd step - if (dir == FORWARD) { - currentstep += MICROSTEPS; - } else { - currentstep -= MICROSTEPS; - } - } - } else if (style == INTERLEAVE) { - if (dir == FORWARD) { - currentstep += MICROSTEPS/2; - } else { - currentstep -= MICROSTEPS/2; - } - } - - if (style == MICROSTEP) { - if (dir == FORWARD) { - currentstep++; - } else { - // BACKWARDS - currentstep--; - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - - ocra = ocrb = 0; - if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) { - ocra = microstepcurve[MICROSTEPS - currentstep]; - ocrb = microstepcurve[currentstep]; - } else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) { - ocra = microstepcurve[currentstep - MICROSTEPS]; - ocrb = microstepcurve[MICROSTEPS*2 - currentstep]; - } else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) { - ocra = microstepcurve[MICROSTEPS*3 - currentstep]; - ocrb = microstepcurve[currentstep - MICROSTEPS*2]; - } else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) { - ocra = microstepcurve[currentstep - MICROSTEPS*3]; - ocrb = microstepcurve[MICROSTEPS*4 - currentstep]; - } - } - - currentstep += MICROSTEPS*4; - currentstep %= MICROSTEPS*4; - -#ifdef MOTORDEBUG - Serial.print("current step: "); Serial.println(currentstep, DEC); - Serial.print(" pwmA = "); Serial.print(ocra, DEC); - Serial.print(" pwmB = "); Serial.println(ocrb, DEC); -#endif - - if (steppernum == 1) { - setPWM1(ocra); - setPWM2(ocrb); - } else if (steppernum == 2) { - setPWM3(ocra); - setPWM4(ocrb); - } - - - // release all - latch_state &= ~a & ~b & ~c & ~d; // all motor pins to 0 - - //Serial.println(step, DEC); - if (style == MICROSTEP) { - if ((currentstep >= 0) && (currentstep < MICROSTEPS)) - latch_state |= a | b; - if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) - latch_state |= b | c; - if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) - latch_state |= c | d; - if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) - latch_state |= d | a; - } else { - switch (currentstep/(MICROSTEPS/2)) { - case 0: - latch_state |= a; // energize coil 1 only - break; - case 1: - latch_state |= a | b; // energize coil 1+2 - break; - case 2: - latch_state |= b; // energize coil 2 only - break; - case 3: - latch_state |= b | c; // energize coil 2+3 - break; - case 4: - latch_state |= c; // energize coil 3 only - break; - case 5: - latch_state |= c | d; // energize coil 3+4 - break; - case 6: - latch_state |= d; // energize coil 4 only - break; - case 7: - latch_state |= d | a; // energize coil 1+4 - break; - } - } - - - MC.latch_tx(); - return currentstep; -} - diff --git a/vehicle_interface/main/UCMotor.h b/vehicle_interface/main/UCMotor.h deleted file mode 100644 index d90ca4ae..00000000 --- a/vehicle_interface/main/UCMotor.h +++ /dev/null @@ -1,195 +0,0 @@ -/** - * @file UCMotor.h - * @author - * @brief This was fetched from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car - * - * @note No edits have been made to to this file aside from the brief - * - */ -// UCTRONICS Motor shield library -// this code is public domain, enjoy! - -/* - * Usage Notes: - * For PIC32, all features work properly with the following two exceptions: - * - * 1) Because the PIC32 only has 5 PWM outputs, and the UCMotor shield needs 6 - * to completely operate (four for motor outputs and two for RC servos), the - * M1 motor output will not have PWM ability when used with a PIC32 board. - * However, there is a very simple workaround. If you need to drive a stepper - * or DC motor with PWM on motor output M1, you can use the PWM output on pin - * 9 or pin 10 (normally use for RC servo outputs on Arduino, not needed for - * RC servo outputs on PIC32) to drive the PWM input for M1 by simply putting - * a jumber from pin 9 to pin 11 or pin 10 to pin 11. Then uncomment one of the - * two #defines below to activate the PWM on either pin 9 or pin 10. You will - * then have a fully functional microstepping for 2 stepper motors, or four - * DC motor outputs with PWM. - * - * 2) There is a conflict between RC Servo outputs on pins 9 and pins 10 and - * the operation of DC motors and stepper motors as of 9/2012. This issue - * will get fixed in future MPIDE releases, but at the present time it means - * that the Motor Party example will NOT work properly. Any time you attach - * an RC servo to pins 9 or pins 10, ALL PWM outputs on the whole board will - * stop working. Thus no steppers or DC motors. - * - */ -// 09/15/2012 Modified for use with chipKIT boards - - -#ifndef _UCMotor_h_ -#define _UCMotor_h_ - -#include -#if defined(__AVR__) - #include - - //#define MOTORDEBUG 1 - - #define MICROSTEPS 16 // 8 or 16 - - #define MOTOR12_64KHZ _BV(CS20) // no prescale - #define MOTOR12_8KHZ _BV(CS21) // divide by 8 - #define MOTOR12_2KHZ _BV(CS21) | _BV(CS20) // divide by 32 - #define MOTOR12_1KHZ _BV(CS22) // divide by 64 - - #define MOTOR34_64KHZ _BV(CS00) // no prescale - #define MOTOR34_8KHZ _BV(CS01) // divide by 8 - #define MOTOR34_1KHZ _BV(CS01) | _BV(CS00) // divide by 64 - - #define DC_MOTOR_PWM_RATE MOTOR34_8KHZ // PWM rate for DC motors - #define STEPPER1_PWM_RATE MOTOR12_64KHZ // PWM rate for stepper 1 - #define STEPPER2_PWM_RATE MOTOR34_64KHZ // PWM rate for stepper 2 - -#elif defined(__PIC32MX__) - //#define MOTORDEBUG 1 - - // Uncomment the one of following lines if you have put a jumper from - // either pin 9 to pin 11 or pin 10 to pin 11 on your Motor Shield. - // Either will enable PWM for M1 - //#define PIC32_USE_PIN9_FOR_M1_PWM - //#define PIC32_USE_PIN10_FOR_M1_PWM - - #define MICROSTEPS 16 // 8 or 16 - - // For PIC32 Timers, define prescale settings by PWM frequency - #define MOTOR12_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR12_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR12_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR12_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR12_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR12_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR12_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR12_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR12_1KHZ 7 // 1:256, actual frequency 1.2KHz - - #define MOTOR34_312KHZ 0 // 1:1, actual frequency 312KHz - #define MOTOR34_156KHZ 1 // 1:2, actual frequency 156KHz - #define MOTOR34_64KHZ 2 // 1:4, actual frequency 78KHz - #define MOTOR34_39KHZ 3 // 1:8, acutal frequency 39KHz - #define MOTOR34_19KHZ 4 // 1:16, actual frequency 19KHz - #define MOTOR34_8KHZ 5 // 1:32, actual frequency 9.7KHz - #define MOTOR34_4_8KHZ 6 // 1:64, actual frequency 4.8KHz - #define MOTOR34_2KHZ 7 // 1:256, actual frequency 1.2KHz - #define MOTOR34_1KHZ 7 // 1:256, actual frequency 1.2KHz - - // PWM rate for DC motors. - #define DC_MOTOR_PWM_RATE MOTOR34_39KHZ - // Note: for PIC32, both of these must be set to the same value - // since there's only one timebase for all 4 PWM outputs - #define STEPPER1_PWM_RATE MOTOR12_39KHZ - #define STEPPER2_PWM_RATE MOTOR34_39KHZ - -#endif - -// Bit positions in the 74HCT595 shift register output -#define MOTOR1_A 2 -#define MOTOR1_B 3 -#define MOTOR2_A 1 -#define MOTOR2_B 4 -#define MOTOR4_A 0 -#define MOTOR4_B 6 -#define MOTOR3_A 5 -#define MOTOR3_B 7 - -// Constants that the user passes in to the motor calls -#define FORWARD 1 -#define BACKWARD 2 -#define LEFT 3 -#define RIGHT 4 -#define STOP 5 - -#define FORWARD2 2 -#define BACKWARD2 1 - -#define BRAKE 3 -#define RELEASE 4 - -// Constants that the user passes in to the stepper calls -#define SINGLE 1 -#define DOUBLE 2 -#define INTERLEAVE 3 -#define MICROSTEP 4 - -/* -#define LATCH 4 -#define LATCH_DDR DDRB -#define LATCH_PORT PORTB - -#define CLK_PORT PORTD -#define CLK_DDR DDRD -#define CLK 4 - -#define ENABLE_PORT PORTD -#define ENABLE_DDR DDRD -#define ENABLE 7 - -#define SER 0 -#define SER_DDR DDRB -#define SER_PORT PORTB -*/ - -// Arduino pin names for interface to 74HCT595 latch -#define MOTORLATCH 12 -#define MOTORCLK 4 -#define MOTORENABLE 7 -#define MOTORDATA 8 - -class UCMotorController -{ - public: - UCMotorController(void); - void enable(void); - friend class UC_DCMotor; - void latch_tx(void); - uint8_t TimerInitalized; -}; - -class UC_DCMotor -{ - public: - UC_DCMotor(uint8_t motornum, uint8_t freq = DC_MOTOR_PWM_RATE); - void run(uint8_t); - void setSpeed(uint8_t); - - private: - uint8_t motornum, pwmfreq; -}; - -class UC_Stepper { - public: - UC_Stepper(uint16_t, uint8_t); - void step(uint16_t steps, uint8_t dir, uint8_t style = SINGLE); - void setSpeed(uint16_t); - uint8_t onestep(uint8_t dir, uint8_t style); - void release(void); - uint16_t revsteps; // # steps per revolution - uint8_t steppernum; - uint32_t usperstep, steppingcounter; - private: - uint8_t currentstep; - -}; - -uint8_t getlatchstate(void); - -#endif diff --git a/vehicle_interface/main/main.ino b/vehicle_interface/main/main.ino index 3554be29..611c4e33 100644 --- a/vehicle_interface/main/main.ino +++ b/vehicle_interface/main/main.ino @@ -64,8 +64,8 @@ void sendCommandMini(UC_DCMotor * uc_motor, wheel_motor_command_t command) { void loop() { - int distance = read_the_sensor_example(); - if (distance > 25){ + int distance = read_ultrasonic_sensor(); + if (distance > MIN_DISTANCE){ // Update wheel commands commands = fetch_rc_commands(); } @@ -80,9 +80,9 @@ void loop() } ////////////////////////////////////////////////////////// -// Code Example: Read the sensor at the default address // +// Read the sensor at the default address // ////////////////////////////////////////////////////////// -int read_the_sensor_example(){ +int read_ultrasonic_sensor(){ boolean error = 0; //Create a bit to check for catch errors as needed. int distance; diff --git a/vehicle_interface/main/rc_commands.h b/vehicle_interface/main/rc_commands.h index 713de962..2076b192 100644 --- a/vehicle_interface/main/rc_commands.h +++ b/vehicle_interface/main/rc_commands.h @@ -1,28 +1,30 @@ -#ifndef RC_COMMANDS_H -#define RC_COMMANDS_H -#include "pinout.h" -#include "wheel.h" -#include "util.h" - -/* Define the max and min thresholds for the longitudinal wheel sets */ -// CH2 - right longitudinal wheel set -#define RC_RIGHT_SET_FW_MAX 1985 -#define RC_RIGHT_SET_FW_MIN 1638 -#define RC_RIGHT_SET_BW_MAX 1310 -#define RC_RIGHT_SET_BW_MIN 980 -// CH3 - left longitudinal wheel set -#define RC_LEFT_SET_FW_MAX 1972 -#define RC_LEFT_SET_FW_MIN 1632 -#define RC_LEFT_SET_BW_MAX 1320 -#define RC_LEFT_SET_BW_MIN 997//1007 -/* Define channels and thresholds for the switches */ -// CH5/6 - SWA/B -#define RC_SWX_LOW_MAX 1000//994 -#define RC_SWX_LOW_MIN 950//987 -#define RC_SWX_HIGH_MAX 2000//1981 -#define RC_SWX_HIGH_MIN 1900//1974 - -/* Function prototypes for rc commands */ -Array fetch_rc_commands(void); - -#endif // !RC_COMMANDS_H \ No newline at end of file +#ifndef RC_COMMANDS_H +#define RC_COMMANDS_H +#include "pinout.h" +#include "wheel.h" +#include "util.h" + +/* Define the max and min thresholds for the longitudinal wheel sets */ +// CH2 - right longitudinal wheel set +#define RC_RIGHT_SET_FW_MAX 1985 +#define RC_RIGHT_SET_FW_MIN 1638 +#define RC_RIGHT_SET_BW_MAX 1310 +#define RC_RIGHT_SET_BW_MIN 980 +// CH3 - left longitudinal wheel set +#define RC_LEFT_SET_FW_MAX 1972 +#define RC_LEFT_SET_FW_MIN 1632 +#define RC_LEFT_SET_BW_MAX 1320 +#define RC_LEFT_SET_BW_MIN 997//1007 +/* Define channels and thresholds for the switches */ +// CH5/6 - SWA/B +#define RC_SWX_LOW_MAX 1000//994 +#define RC_SWX_LOW_MIN 950//987 +#define RC_SWX_HIGH_MAX 2000//1981 +#define RC_SWX_HIGH_MIN 1900//1974 + +/* Define the minimum distance that the ultrasonic sensor can detect*/ +#define MIN_DISTANCE 25 +/* Function prototypes for rc commands */ +Array fetch_rc_commands(void); + +#endif // !RC_COMMANDS_H From 956800402484f18720969c2e87899379dc5bd4e2 Mon Sep 17 00:00:00 2001 From: zhaoshengEE <43089911+zhaoshengEE@users.noreply.github.com> Date: Tue, 7 Apr 2020 18:27:49 -0700 Subject: [PATCH 6/6] Update README.md --- vehicle_interface/main/README.md | 34 +++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/vehicle_interface/main/README.md b/vehicle_interface/main/README.md index 523d244b..b32dfd08 100644 --- a/vehicle_interface/main/README.md +++ b/vehicle_interface/main/README.md @@ -1,5 +1,7 @@ # PropBot +## Function Library + An I2C library is necessary for this project. Please download the SoftI2CMaster.h from the following github link: https://github.com/felias-fogg/SoftI2CMaster @@ -8,4 +10,34 @@ And install the library onto Arduino IDE. For how to install library on Arduino IDE: https://www.arduino.cc/en/Guide/Libraries -Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car \ No newline at end of file +Please also download UCMotor.h library file and UCMotor.cpp file from https://github.com/UCTRONICS/Smart-Robot-Car-Arduino/tree/master/UCTRONICS_Smart_Robot_Car + +## Remote Controller (FS-T6) + +### Left Joystick + +Forward: Left sets of wheels rotate forward + +Backward: Left sets of wheels rotate backward + +### Right Joystick + +Forward: Right sets of wheels rotate forward + +Backward: Right sets of wheels rotate backward + +### Switch A + +0: E-stop deactivated + +1: E-stop activated, the car will shut down + +### Switch B + +0: Manual control mode + +1: Autonomy mode + +## Binding RC receiver with transmitter + +Plesae refer to this tutorial video: https://www.youtube.com/watch?v=7bX7M6E8U0M