From: Andy Spencer Date: Fri, 31 Jan 2014 05:27:13 +0000 (+0000) Subject: Add initial hw1 stuff X-Git-Url: http://pileus.org/git/?p=~andy%2Fcsm213a-hw;a=commitdiff_plain;h=1025b8971dde3517c15107fcf0d8e9848ac3e03f Add initial hw1 stuff --- 1025b8971dde3517c15107fcf0d8e9848ac3e03f diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8764137 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +*.bin +*.elf +*.o +*~ +config.mk diff --git a/common.mk b/common.mk new file mode 100644 index 0000000..710c645 --- /dev/null +++ b/common.mk @@ -0,0 +1,68 @@ +# Settings +GDB ?= arm-none-eabi-gdb +GAS ?= arm-none-eabi-as +GCC ?= arm-none-eabi-gcc +GXX ?= arm-none-eabi-g++ +GLD ?= arm-none-eabi-gcc +OBJCOPY ?= arm-none-eabi-objcopy + +CFLAGS ?= -Wall -Os -g --std=gnu99 +CXXFLAGS ?= -Wall -Os -g --std=gnu++98 + +# Cross compiler flags +CPPFLAGS += -I$(MBED) -I$(MBED)/TARGET_KL46Z + +CFLAGS += -mthumb -mcpu=cortex-m0plus +CXXFLAGS += -mthumb -mcpu=cortex-m0plus -fno-exceptions + +LDFLAGS += --specs=nano.specs -Wl,--gc-sections \ + -T$(MBED)/TARGET_KL46Z/TOOLCHAIN_GCC_ARM/MKL46Z4.ld \ + -L$(MBED)/TARGET_KL46Z/TOOLCHAIN_GCC_ARM \ + -Wl,--start-group -lmbed -lstartup -lio -lc_s -Wl,--end-group + +# Targets +all: $(PROG).bin + +info: all + @echo "LS $(PROG).bin $(PROG).elf" + @ls -lh $(PROG).bin $(PROG).elf + +install: all + @echo "INST $(PROG).bin" + @if [ -b /dev/sdb ]; then \ + mount /mnt/usb; \ + cp $(PROG).bin /mnt/usb; \ + umount /mnt/usb; \ + else \ + echo Error: No USB found; \ + fi + +gdb: all + $(GDB) -ex 'target remote localhost:3333' $(PROG).elf + +openocd: all + openocd -c 'adapter_khz 5000' \ + -c 'gdb_memory_map disable' \ + -s /usr/share/openocd/scripts \ + -f board/frdm-kl46z.cfg + +clean: + @echo "RM $(PROG).bin $(PROG).elf $(OBJS)" + @rm -f $(PROG).bin $(PROG).elf $(OBJS) + +# Rules +%.o: %.c makefile ../common.mk ../config.mk + @echo "CC $<" + @$(GCC) $(CFLAGS) $(CPPFLAGS) -c -o $@ $< + +%.o: %.cc makefile ../common.mk ../config.mk + @echo "CXX $<" + @$(GXX) $(CXXFLAGS) $(CPPFLAGS) -c -o $@ $< + +$(PROG).elf: $(OBJS) + @echo "LD $+" + @$(GLD) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +$(PROG).bin: $(PROG).elf + @echo "BIN $+" + @$(OBJCOPY) -O binary $< $@ diff --git a/config.mk.example b/config.mk.example new file mode 100644 index 0000000..3c9ab5a --- /dev/null +++ b/config.mk.example @@ -0,0 +1,2 @@ +PATH := $(PATH):/home/andy/class/csm213a/arm/bin +MBED := /home/andy/class/csm213a/mbed/build/mbed diff --git a/hw1/acc.cc b/hw1/acc.cc new file mode 100644 index 0000000..f398675 --- /dev/null +++ b/hw1/acc.cc @@ -0,0 +1,81 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#include "acc.h" + +#define REG_WHO_AM_I 0x0D +#define REG_CTRL_REG_1 0x2A +#define REG_OUT_X_MSB 0x01 +#define REG_OUT_Y_MSB 0x03 +#define REG_OUT_Z_MSB 0x05 + +#define UINT14_MAX 16383 + +MMA8451Q::MMA8451Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) { + // activate the peripheral + uint8_t data[2] = {REG_CTRL_REG_1, 0x01}; + writeRegs(data, 2); +} + +MMA8451Q::~MMA8451Q() { } + +uint8_t MMA8451Q::getWhoAmI() { + uint8_t who_am_i = 0; + readRegs(REG_WHO_AM_I, &who_am_i, 1); + return who_am_i; +} + +int16_t MMA8451Q::getAccX() { + return getAccAxis(REG_OUT_X_MSB); +} + +int16_t MMA8451Q::getAccY() { + return getAccAxis(REG_OUT_Y_MSB); +} + +int16_t MMA8451Q::getAccZ() { + return getAccAxis(REG_OUT_Z_MSB); +} + +void MMA8451Q::getAccAllAxis(int16_t * res) { + res[0] = getAccX(); + res[1] = getAccY(); + res[2] = getAccZ(); +} + +int16_t MMA8451Q::getAccAxis(uint8_t addr) { + int16_t acc; + uint8_t res[2]; + readRegs(addr, res, 2); + + acc = (res[0] << 6) | (res[1] >> 2); + if (acc > UINT14_MAX/2) + acc -= UINT14_MAX; + + return acc; +} + +void MMA8451Q::readRegs(int addr, uint8_t * data, int len) { + char t[1] = {(char)addr}; + m_i2c.write(m_addr, t, 1, true); + m_i2c.read(m_addr, (char *)data, len); +} + +void MMA8451Q::writeRegs(uint8_t * data, int len) { + m_i2c.write(m_addr, (char *)data, len); +} diff --git a/hw1/acc.h b/hw1/acc.h new file mode 100644 index 0000000..1b22268 --- /dev/null +++ b/hw1/acc.h @@ -0,0 +1,108 @@ +/* Copyright (c) 2010-2011 mbed.org, MIT License +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software +* and associated documentation files (the "Software"), to deal in the Software without +* restriction, including without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the +* Software is furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in all copies or +* substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING +* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, +* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#ifndef MMA8451Q_H +#define MMA8451Q_H + +#include "mbed.h" + +/** +* MMA8451Q accelerometer example +* #include "mbed.h" +* #include "MMA8451Q.h" +* +* #define MMA8451_I2C_ADDRESS (0x1d<<1) +* +* int main(void) { +* DigitalOut led(LED_GREEN); +* MMA8451Q acc(P_E25, P_E24, MMA8451_I2C_ADDRESS); +* printf("WHO AM I: 0x%2X\r\n", acc.getWhoAmI()); +* +* while (true) { +* printf("-----------\r\n"); +* printf("acc_x: %d\r\n", acc.getAccX()); +* printf("acc_y: %d\r\n", acc.getAccY()); +* printf("acc_z: %d\r\n", acc.getAccZ()); +* +* wait(1); +* led = !led; +* } +* } +*/ +class MMA8451Q +{ +public: + /** + * MMA8451Q constructor + * + * @param sda SDA pin + * @param sdl SCL pin + * @param addr addr of the I2C peripheral + */ + MMA8451Q(PinName sda, PinName scl, int addr); + + /** + * MMA8451Q destructor + */ + ~MMA8451Q(); + + /** + * Get the value of the WHO_AM_I register + * + * @returns WHO_AM_I value + */ + uint8_t getWhoAmI(); + + /** + * Get X axis acceleration + * + * @returns X axis acceleration + */ + int16_t getAccX(); + + /** + * Get Y axis acceleration + * + * @returns Y axis acceleration + */ + int16_t getAccY(); + + /** + * Get Z axis acceleration + * + * @returns Z axis acceleration + */ + int16_t getAccZ(); + + /** + * Get XYZ axis acceleration + * + * @param res array where acceleration data will be stored + */ + void getAccAllAxis(int16_t * res); + +private: + I2C m_i2c; + int m_addr; + void readRegs(int addr, uint8_t * data, int len); + void writeRegs(uint8_t * data, int len); + int16_t getAccAxis(uint8_t addr); + +}; + +#endif diff --git a/hw1/assignment.pdf b/hw1/assignment.pdf new file mode 100644 index 0000000..e581b13 Binary files /dev/null and b/hw1/assignment.pdf differ diff --git a/hw1/mag.cc b/hw1/mag.cc new file mode 100644 index 0000000..a379bf2 --- /dev/null +++ b/hw1/mag.cc @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "mag.h" + +/****************************************************************************** + * Constructors + ******************************************************************************/ +MAG3110::MAG3110(PinName sda, PinName scl, int addr): _i2c(sda, scl), + _i2c_address(addr), _pc(NULL), _debug(false) +{ + begin(); +} + +MAG3110::MAG3110(PinName sda, PinName scl, int addr, Serial *pc): _i2c(sda, scl), + _i2c_address(addr), _pc(pc), _debug(true) +{ + begin(); +} + +void MAG3110::begin() +{ + char cmd[2]; + + cmd[0] = MAG_CTRL_REG2; + cmd[1] = 0x80; + _i2c.write(_i2c_address, cmd, 2); + + cmd[0] = MAG_CTRL_REG1; + cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; + _i2c.write(_i2c_address, cmd, 2); + + // No adjustment initially + _avgX = 0; + _avgY = 0; +} + +// Read a single byte form 8 bit register, return as int +int MAG3110::readReg(char regAddr) +{ + char cmd[1]; + + cmd[0] = regAddr; + _i2c.write(_i2c_address, cmd, 1); + + cmd[0] = 0x00; + _i2c.read(_i2c_address, cmd, 1); + return (int)( cmd[0]); +} + + +// read a register per, pass first reg value, reading 2 bytes increments register +// Reads MSB first then LSB +int MAG3110::readVal(char regAddr) +{ + char cmd[2]; + + cmd[0] = regAddr; + _i2c.write(_i2c_address, cmd, 1); + + cmd[0] = 0x00; + cmd[1] = 0x00; + _i2c.read(_i2c_address, cmd, 2); + return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB +} + + +float MAG3110::getHeading() +{ + int xVal = readVal(MAG_OUT_X_MSB); + int yVal = readVal(MAG_OUT_Y_MSB); + return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI; +} + +void MAG3110::getValues(int *xVal, int *yVal, int *zVal) +{ + *xVal = readVal(MAG_OUT_X_MSB); + *yVal = readVal(MAG_OUT_Y_MSB); + *zVal = readVal(MAG_OUT_Z_MSB); +} + +int16_t MAG3110::getMagX() +{ + return readVal(MAG_OUT_X_MSB); +} + +int16_t MAG3110::getMagY() +{ + return readVal(MAG_OUT_Y_MSB); +} + +int16_t MAG3110::getMagZ() +{ + return readVal(MAG_OUT_Z_MSB); +} + +void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY ) +{ + _avgX=(maxX+minX)/2; + _avgY=(maxY+minY)/2; +} + + + + + diff --git a/hw1/mag.h b/hw1/mag.h new file mode 100644 index 0000000..7ddbde6 --- /dev/null +++ b/hw1/mag.h @@ -0,0 +1,160 @@ +/* + * MAG3110 Sensor Library for mbed + * TODO: Add proper header + */ + +#ifndef MAG3110_H +#define MAG3110_H + +#include "mbed.h" + +#define PI 3.14159265359 + +// define registers +#define MAG_DR_STATUS 0x00 +#define MAG_OUT_X_MSB 0x01 +#define MAG_OUT_X_LSB 0x02 +#define MAG_OUT_Y_MSB 0x03 +#define MAG_OUT_Y_LSB 0x04 +#define MAG_OUT_Z_MSB 0x05 +#define MAG_OUT_Z_LSB 0x06 +#define MAG_WHO_AM_I 0x07 +#define MAG_SYSMOD 0x08 +#define MAG_OFF_X_MSB 0x09 +#define MAG_OFF_X_LSB 0x0A +#define MAG_OFF_Y_MSB 0x0B +#define MAG_OFF_Y_LSB 0x0C +#define MAG_OFF_Z_MSB 0x0D +#define MAG_OFF_Z_LSB 0x0E +#define MAG_DIE_TEMP 0x0F +#define MAG_CTRL_REG1 0x10 +#define MAG_CTRL_REG2 0x11 + +// what should WHO_AM_I return? +#define MAG_3110_WHO_AM_I_VALUE 0xC4 + + +// Fields in registers +// CTRL_REG1: dr2,dr1,dr0 os1,os0 fr tm ac + +// Sampling rate from 80Hz down to 0.625Hz +#define MAG_3110_SAMPLE80 0 +#define MAG_3110_SAMPLE40 0x20 +#define MAG_3110_SAMPLE20 0x40 +#define MAG_3110_SAMPLE10 0x60 +#define MAG_3110_SAMPLE5 0x80 +#define MAG_3110_SAMPLE2_5 0xA0 +#define MAG_3110_SAMPLE1_25 0xC0 +#define MAG_3110_SAMPLE0_625 0xE0 + +// How many samples to average (lowers data rate) +#define MAG_3110_OVERSAMPLE1 0 +#define MAG_3110_OVERSAMPLE2 0x08 +#define MAG_3110_OVERSAMPLE3 0x10 +#define MAG_3110_OVERSAMPLE4 0x18 + +// read only 1 byte per axis +#define MAG_3110_FASTREAD 0x04 +// do one measurement (even if in standby mode) +#define MAG_3110_TRIGGER 0x02 +// put in active mode +#define MAG_3110_ACTIVE 0x01 + +// CTRL_REG2: AUTO_MRST_EN _ RAW MAG_RST _ _ _ _ _ +// reset sensor after each reading +#define MAG_3110_AUTO_MRST_EN 0x80 +// don't subtract user offsets +#define MAG_3110_RAW 0x20 +// reset magnetic sensor after too-large field +#define MAG_3110_MAG_RST 0x10 + +// DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR +#define MAG_3110_ZYXDR 0x08 + +/** + * MAG3110 Class to read X/Y/Z data from the magentometer + * + */ +class MAG3110 +{ +public: + /** + * Main constructor + * @param sda SDA pin + * @param sdl SCL pin + * @param addr addr of the I2C peripheral + */ + MAG3110(PinName sda, PinName scl, int addr); + /** + * Debug version of constructor + * @param sda SDA pin + * @param sdl SCL pin + * @param addr Address of the I2C peripheral + * @param pc Serial object to output debug messages + */ + MAG3110(PinName sda, PinName scl, int addr, Serial *pc); //pass serial for debug + /** + * Setup the Magnetometer + * + */ + void begin(); + /** + * Read a register, return its value as int + * @param regAddr The address to read + * @return value in register + */ + int readReg(char regAddr); + /** + * Read a value from a pair of registers, return as int + * @param regAddr The address to read + * @return Value from 2 consecutive registers + */ + int readVal(char regAddr); + /** + * Calculate the heading + * @return heading in degrees + */ + float getHeading(); + /** + * Perform a read on the X, Y and Z values. + * @param xVal Pointer to X value + * @param yVal Pointer to Y value + * @param zVal Pointer to Z value + */ + void getValues(int *xVal, int *yVal, int *zVal); + /** + * Set the calibration parameters if required. + * @param minX Minimum value for X range + * @param maxX Maximum value for X range + * @param minY Minimum value for Y range + * @param maxY maximum value for Y range + */ + void setCalibration(int minX, int maxX, int minY, int maxY); + /** + * Get X axis magnetism + * + * @returns X axis magnetism + */ + int16_t getMagX(); + /** + * Get Y axis magnetism + * + * @returns Y axis magnetism + */ + int16_t getMagY(); + /** + * Get Z axis magnetism + * + * @returns Z axis magnetism + */ + int16_t getMagZ(); + +private: + I2C _i2c; + int _i2c_address; + Serial *_pc; + bool _debug; + int _avgX, _avgY; + +}; +#endif diff --git a/hw1/makefile b/hw1/makefile new file mode 100644 index 0000000..9a4e195 --- /dev/null +++ b/hw1/makefile @@ -0,0 +1,9 @@ +PROG = mbed +OBJS = mbed.o tsi.o acc.o mag.o + +LDFLAGS = -lm + +default: info install + +-include ../config.mk +-include ../common.mk diff --git a/hw1/mbed.cc b/hw1/mbed.cc new file mode 100644 index 0000000..645d6da --- /dev/null +++ b/hw1/mbed.cc @@ -0,0 +1,47 @@ +#include "mbed.h" +#include "tsi.h" +#include "acc.h" +#include "mag.h" + +/* Sensors: + * accel [x,y,z] + * compas + * light + * slider + * a2d[6] + */ + +#define ACC_ADDR (0x1D<<1) +#define MAG_ADDR (0x1D) // FIXME + +DigitalOut led1(LED1); +DigitalOut led2(LED2); +MMA8451Q acc(PTE25, PTE24, ACC_ADDR); +MAG3110 mag(PTE25, PTE24, MAG_ADDR); +TSISensor tsi; + +static int state = 0; + +int main(int argc, char **argv) +{ + printf("Starting...\r\n"); + while (1) { + int chr = getchar(); + putchar(chr); + + printf("tsi: [%d]\r\n", + tsi.readDistance()); + printf("acc: [%d,%d,%d]\r\n", + acc.getAccX(), + acc.getAccY(), + acc.getAccZ()); + printf("mag: [%d,%d,%d]\r\n", + mag.getMagX(), + mag.getMagY(), + mag.getMagZ()); + + led2 = state; + state ^= 1; + } + return 0; +} diff --git a/hw1/tsi.cc b/hw1/tsi.cc new file mode 100644 index 0000000..e83775e --- /dev/null +++ b/hw1/tsi.cc @@ -0,0 +1,231 @@ +/* Freescale Semiconductor Inc. + * (c) Copyright 2004-2005 Freescale Semiconductor, Inc. + * (c) Copyright 2001-2004 Motorola, Inc. + * + * mbed Microcontroller Library + * (c) Copyright 2009-2012 ARM Limited. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "mbed.h" +#include "tsi.h" + +#define NO_TOUCH 0 +#define SLIDER_LENGTH 40 //LENGTH in mm +#define TOTAL_ELECTRODE 2 + +#define TSI0a 0 +#define TSI1 1 +#define TSI2 2 +#define TSI3 3 +#define TSI4 4 +#define TSI5 5 +#define TSI6 6 +#define TSI7 7 +#define TSI8 8 +#define TSI9 9 +#define TSI10 10 +#define TSI11 11 +#define TSI12 12 +#define TSI13 13 +#define TSI14 14 +#define TSI15 15 + +/*Chose the correct TSI channel for the electrode number*/ +#define ELECTRODE0 TSI9 +#define ELECTRODE1 TSI10 +#define ELECTRODE2 TSI0a +#define ELECTRODE3 TSI1 +#define ELECTRODE4 TSI2 +#define ELECTRODE5 TSI3 +#define ELECTRODE6 TSI4 +#define ELECTRODE7 TSI5 +#define ELECTRODE8 TSI6 +#define ELECTRODE9 TSI7 +#define ELECTRODE10 TSI8 +#define ELECTRODE11 TSI11 +#define ELECTRODE12 TSI12 +#define ELECTRODE13 TSI13 +#define ELECTRODE14 TSI14 +#define ELECTRODE15 TSI15 + +#define THRESHOLD0 100 +#define THRESHOLD1 100 +#define THRESHOLD2 100 +#define THRESHOLD3 100 +#define THRESHOLD4 100 +#define THRESHOLD5 100 +#define THRESHOLD6 100 +#define THRESHOLD7 100 +#define THRESHOLD8 100 +#define THRESHOLD9 100 +#define THRESHOLD10 100 +#define THRESHOLD11 100 +#define THRESHOLD12 100 +#define THRESHOLD13 100 +#define THRESHOLD14 100 +#define THRESHOLD15 100 + +static uint8_t total_electrode = TOTAL_ELECTRODE; +static uint8_t elec_array[16]={ELECTRODE0,ELECTRODE1,ELECTRODE2,ELECTRODE3,ELECTRODE4,ELECTRODE5, + ELECTRODE6,ELECTRODE7,ELECTRODE8,ELECTRODE9,ELECTRODE10,ELECTRODE11, + ELECTRODE12,ELECTRODE13,ELECTRODE14,ELECTRODE15}; +static uint16_t gu16TSICount[16]; +static uint16_t gu16Baseline[16]; +static uint16_t gu16Threshold[16]={THRESHOLD0,THRESHOLD1,THRESHOLD2,THRESHOLD3,THRESHOLD4,THRESHOLD5, + THRESHOLD6,THRESHOLD7,THRESHOLD8,THRESHOLD9,THRESHOLD10,THRESHOLD11, + THRESHOLD12,THRESHOLD13,THRESHOLD14,THRESHOLD15}; +static uint16_t gu16Delta[16]; +static uint8_t ongoing_elec; +static uint8_t end_flag = 1; + +static uint8_t SliderPercentegePosition[2] = {NO_TOUCH,NO_TOUCH}; +static uint8_t SliderDistancePosition[2] = {NO_TOUCH,NO_TOUCH}; +static uint32_t AbsolutePercentegePosition = NO_TOUCH; +static uint32_t AbsoluteDistancePosition = NO_TOUCH; + +static void tsi_irq(); + +TSISensor::TSISensor() { + SIM->SCGC5 |= SIM_SCGC5_PORTB_MASK; + SIM->SCGC5 |= SIM_SCGC5_TSI_MASK; + + TSI0->GENCS |= (TSI_GENCS_ESOR_MASK + | TSI_GENCS_MODE(0) + | TSI_GENCS_REFCHRG(4) + | TSI_GENCS_DVOLT(0) + | TSI_GENCS_EXTCHRG(7) + | TSI_GENCS_PS(4) + | TSI_GENCS_NSCN(11) + | TSI_GENCS_TSIIEN_MASK + | TSI_GENCS_STPE_MASK + ); + + TSI0->GENCS |= TSI_GENCS_TSIEN_MASK; + + NVIC_SetVector(TSI0_IRQn, (uint32_t)&tsi_irq); + NVIC_EnableIRQ(TSI0_IRQn); + + selfCalibration(); +} + + +void TSISensor::selfCalibration(void) +{ + unsigned char cnt; + unsigned char trigger_backup; + + TSI0->GENCS |= TSI_GENCS_EOSF_MASK; // Clear End of Scan Flag + TSI0->GENCS &= ~TSI_GENCS_TSIEN_MASK; // Disable TSI module + + if(TSI0->GENCS & TSI_GENCS_STM_MASK) // Back-up TSI Trigger mode from Application + trigger_backup = 1; + else + trigger_backup = 0; + + TSI0->GENCS &= ~TSI_GENCS_STM_MASK; // Use SW trigger + TSI0->GENCS &= ~TSI_GENCS_TSIIEN_MASK; // Enable TSI interrupts + + TSI0->GENCS |= TSI_GENCS_TSIEN_MASK; // Enable TSI module + + for(cnt=0; cnt < total_electrode; cnt++) // Get Counts when Electrode not pressed + { + TSI0->DATA = ((elec_array[cnt] << TSI_DATA_TSICH_SHIFT) ); + TSI0->DATA |= TSI_DATA_SWTS_MASK; + while(!(TSI0->GENCS & TSI_GENCS_EOSF_MASK)); + TSI0->GENCS |= TSI_GENCS_EOSF_MASK; + gu16Baseline[cnt] = (TSI0->DATA & TSI_DATA_TSICNT_MASK); + } + + TSI0->GENCS &= ~TSI_GENCS_TSIEN_MASK; // Disable TSI module + TSI0->GENCS |= TSI_GENCS_TSIIEN_MASK; // Enale TSI interrupt + if(trigger_backup) // Restore trigger mode + TSI0->GENCS |= TSI_GENCS_STM_MASK; + else + TSI0->GENCS &= ~TSI_GENCS_STM_MASK; + + TSI0->GENCS |= TSI_GENCS_TSIEN_MASK; // Enable TSI module + + TSI0->DATA = ((elec_array[0]<DATA |= TSI_DATA_SWTS_MASK; +} + + +void TSISensor::sliderRead(void ) { + if(end_flag) { + end_flag = 0; + if((gu16Delta[0] > gu16Threshold[0])||(gu16Delta[1] > gu16Threshold[1])) { + SliderPercentegePosition[0] = (gu16Delta[0]*100)/(gu16Delta[0]+gu16Delta[1]); + SliderPercentegePosition[1] = (gu16Delta[1]*100)/(gu16Delta[0]+gu16Delta[1]); + SliderDistancePosition[0] = (SliderPercentegePosition[0]* SLIDER_LENGTH)/100; + SliderDistancePosition[1] = (SliderPercentegePosition[1]* SLIDER_LENGTH)/100; + AbsolutePercentegePosition = ((100 - SliderPercentegePosition[0]) + SliderPercentegePosition[1])/2; + AbsoluteDistancePosition = ((SLIDER_LENGTH - SliderDistancePosition[0]) + SliderDistancePosition[1])/2; + } else { + SliderPercentegePosition[0] = NO_TOUCH; + SliderPercentegePosition[1] = NO_TOUCH; + SliderDistancePosition[0] = NO_TOUCH; + SliderDistancePosition[1] = NO_TOUCH; + AbsolutePercentegePosition = NO_TOUCH; + AbsoluteDistancePosition = NO_TOUCH; + } + } +} + +float TSISensor::readPercentage() { + sliderRead(); + return (float)AbsolutePercentegePosition/100.0; +} + +uint8_t TSISensor::readDistance() { + sliderRead(); + return AbsoluteDistancePosition; +} + + +static void changeElectrode(void) +{ + int16_t u16temp_delta; + + gu16TSICount[ongoing_elec] = (TSI0->DATA & TSI_DATA_TSICNT_MASK); // Save Counts for current electrode + u16temp_delta = gu16TSICount[ongoing_elec] - gu16Baseline[ongoing_elec]; // Obtains Counts Delta from callibration reference + if(u16temp_delta < 0) + gu16Delta[ongoing_elec] = 0; + else + gu16Delta[ongoing_elec] = u16temp_delta; + + //Change Electrode to Scan + if(total_electrode > 1) + { + if((total_electrode-1) > ongoing_elec) + ongoing_elec++; + else + ongoing_elec = 0; + + TSI0->DATA = ((elec_array[ongoing_elec]<DATA |= TSI_DATA_SWTS_MASK; + } +} + + +void tsi_irq(void) +{ + end_flag = 1; + TSI0->GENCS |= TSI_GENCS_EOSF_MASK; // Clear End of Scan Flag + changeElectrode(); +} + diff --git a/hw1/tsi.h b/hw1/tsi.h new file mode 100644 index 0000000..1ac547b --- /dev/null +++ b/hw1/tsi.h @@ -0,0 +1,73 @@ +/* Freescale Semiconductor Inc. + * (c) Copyright 2004-2005 Freescale Semiconductor, Inc. + * (c) Copyright 2001-2004 Motorola, Inc. + * + * mbed Microcontroller Library + * (c) Copyright 2009-2012 ARM Limited. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef TSISENSOR_H +#define TSISENSOR_H + +/** +* TSISensor example +* +* @code +* #include "mbed.h" +* #include "TSISensor.h" +* +* int main(void) { +* DigitalOut led(LED_GREEN); +* TSISensor tsi; +* +* while (true) { +* printf("slider percentage: %f%\r\n", tsi.readPercentage()); +* printf("slider distance: %dmm\r\n", tsi.readDistance()); +* wait(1); +* led = !led; +* } +* } +* @endcode +*/ +class TSISensor { +public: + /** + * Initialize the TSI Touch Sensor + */ + TSISensor(); + + /** + * Read Touch Sensor percentage value + * + * @returns percentage value between [0 ... 1] + */ + float readPercentage(); + + /** + * Read Touch Sensor distance + * + * @returns distance in mm. The value is between [0 ... 40] + */ + uint8_t readDistance(); + +private: + void sliderRead(void); + void selfCalibration(void); +}; + +#endif