Revision 0:e8eecd4b9a3d, committed 2018-12-06

Comitter:

amitchell41

Date:

Thu Dec 06 01:30:51 2018 +0000

Commit message:

Swing batta batta swing

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8452.cpp	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,538 @@
+// Authors: Ashley Mills, Nicholas Herriot
+/* Copyright (c) 2013 Vodafone, 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 "MMA8452.h"
+#include "mbed.h"
+
+#ifdef MMA8452_DEBUG
+// you need to define Serial pc(USBTX,USBRX) somewhere for the below line to make sense
+extern Serial pc;
+#define MMA8452_DBG(...) pc.printf(__VA_ARGS__); pc.printf("\r\n");
+#else
+#define MMA8452_DBG(...)
+#endif
+
+// Connect module at I2C address using I2C port pins sda and scl
+MMA8452::MMA8452(PinName sda, PinName scl, int frequency) : _i2c(sda, scl) , _frequency(frequency) {
+   MMA8452_DBG("Creating MMA8452");
+   
+   // set I2C frequency
+   _i2c.frequency(_frequency);
+   
+   // setup read and write addresses for convenience
+   _readAddress   = MMA8452_ADDRESS | 0x01;
+   _writeAddress  = MMA8452_ADDRESS & 0xFE;
+   
+   // set some defaults
+   _bitDepth = BIT_DEPTH_UNKNOWN;
+   setBitDepth(BIT_DEPTH_12);
+   _dynamicRange = DYNAMIC_RANGE_UNKNOWN;
+   setDynamicRange(DYNAMIC_RANGE_2G);
+   
+   MMA8452_DBG("Done");
+}
+
+
+// Destroys instance
+MMA8452::~MMA8452() {}
+
+// Setting the control register bit 1 to true to activate the MMA8452
+int MMA8452::activate() {
+    // perform write and return error code
+    return logicalORRegister(MMA8452_CTRL_REG_1,MMA8452_ACTIVE_MASK);
+}
+
+// Setting the control register bit 1 to 0 to standby the MMA8452
+int MMA8452::standby() {
+    // perform write and return error code
+    return logicalANDRegister(MMA8452_CTRL_REG_1,MMA8452_STANDBY_MASK);
+}
+
+// this reads a register, applies a bitmask with logical AND, sets a value with logical OR,
+// and optionally goes into and out of standby at the beginning and end of the function respectively
+int MMA8452::maskAndApplyRegister(char reg, char mask, char value, int toggleActivation) {
+   if(toggleActivation) {
+       if(standby()) {
+          return 1;
+       }
+   }
+   
+   // read from register
+   char oldValue = 0;
+   if(readRegister(reg,&oldValue)) {
+      return 1;
+   }
+   
+   // apply bitmask
+   oldValue &= mask;
+   
+   // set value
+   oldValue |= value;
+   
+   // write back to register
+   if(writeRegister(reg,oldValue)) {
+      return 1;
+   }
+   
+   if(toggleActivation) {
+       if(activate()) {
+          return 1;
+       }
+   }
+   return 0;
+}
+
+int MMA8452::setDynamicRange(DynamicRange range, int toggleActivation) {
+   _dynamicRange = range;
+   return maskAndApplyRegister(
+      MMA8452_XYZ_DATA_CFG,
+      MMA8452_DYNAMIC_RANGE_MASK,
+      range,
+      toggleActivation
+   );
+}
+
+int MMA8452::setDataRate(DataRateHz dataRate, int toggleActivation) {
+   return maskAndApplyRegister(
+       MMA8452_CTRL_REG_1,
+       MMA8452_DATA_RATE_MASK,
+       dataRate<<MMA8452_DATA_RATE_MASK_SHIFT,
+       toggleActivation
+   );
+}
+
+int MMA8452::setBitDepth(BitDepth depth,int toggleActivation) {
+   _bitDepth = depth;
+   return maskAndApplyRegister(
+      MMA8452_CTRL_REG_1,
+      MMA8452_BIT_DEPTH_MASK,
+      depth<<MMA8452_BIT_DEPTH_MASK_SHIFT,
+      toggleActivation
+   );
+}
+
+char MMA8452::getMaskedRegister(int addr, char mask) {
+   char rval = 0;
+   if(readRegister(addr,&rval)) {
+      return 0;
+   }
+   return (rval&mask);
+}
+
+int MMA8452::isXYZReady() {
+   return getMaskedRegister(MMA8452_STATUS,MMA8452_STATUS_ZYXDR_MASK)>0;
+}
+
+int MMA8452::isXReady() {
+   return getMaskedRegister(MMA8452_STATUS,MMA8452_STATUS_XDR_MASK)>0;
+}
+
+int MMA8452::isYReady() {
+   return getMaskedRegister(MMA8452_STATUS,MMA8452_STATUS_YDR_MASK)>0;
+}
+
+int MMA8452::isZReady() {
+   return getMaskedRegister(MMA8452_STATUS,MMA8452_STATUS_ZDR_MASK)>0;
+}
+
+
+int MMA8452::getDeviceID(char *dst) {
+   return readRegister(MMA8452_WHO_AM_I,dst);
+}
+
+int MMA8452::getStatus(char* dst) {
+   return readRegister(MMA8452_STATUS,dst);
+}
+
+MMA8452::DynamicRange MMA8452::getDynamicRange() {
+   char rval = 0;
+   if(readRegister(MMA8452_XYZ_DATA_CFG,&rval)) {
+      return MMA8452::DYNAMIC_RANGE_UNKNOWN;
+   }
+   rval &= (MMA8452_DYNAMIC_RANGE_MASK^0xFF);
+   return (MMA8452::DynamicRange)rval;
+}
+
+MMA8452::DataRateHz MMA8452::getDataRate() {
+   char rval = 0;
+   if(readRegister(MMA8452_CTRL_REG_1,&rval)) {
+      return MMA8452::RATE_UNKNOWN;
+   }
+   // logical AND with inverse of mask
+   rval = rval&(MMA8452_DATA_RATE_MASK^0xFF);
+   // shift back into position
+   rval >>= MMA8452_DATA_RATE_MASK_SHIFT;
+   return (MMA8452::DataRateHz)rval;
+}
+
+// Reads xyz
+int MMA8452::readXYZRaw(char *dst) {
+   if(_bitDepth==BIT_DEPTH_UNKNOWN) {
+      return 1;
+   }
+   int readLen = 3;
+   if(_bitDepth==BIT_DEPTH_12) {
+      readLen = 6;
+   }
+   return readRegister(MMA8452_OUT_X_MSB,dst,readLen);
+}
+
+int MMA8452::readXRaw(char *dst) {
+   if(_bitDepth==BIT_DEPTH_UNKNOWN) {
+      return 1;
+   }
+   int readLen = 1;
+   if(_bitDepth==BIT_DEPTH_12) {
+      readLen = 2;
+   }
+   return readRegister(MMA8452_OUT_X_MSB,dst,readLen);
+}
+
+int MMA8452::readYRaw(char *dst) {
+   if(_bitDepth==BIT_DEPTH_UNKNOWN) {
+      return 1;
+   }
+   int readLen = 1;
+   if(_bitDepth==BIT_DEPTH_12) {
+      readLen = 2;
+   }
+   return readRegister(MMA8452_OUT_Y_MSB,dst,readLen);
+}
+
+int MMA8452::readZRaw(char *dst) {
+   if(_bitDepth==BIT_DEPTH_UNKNOWN) {
+      return 1;
+   }
+   int readLen = 1;
+   if(_bitDepth==BIT_DEPTH_12) {
+      readLen = 2;
+   }
+   return readRegister(MMA8452_OUT_Z_MSB,dst,readLen);
+}
+
+int MMA8452::readXYZCounts(int *x, int *y, int *z) {
+   char buf[6];
+   if(readXYZRaw((char*)&buf)) {
+       return 1;
+   }
+   if(_bitDepth==BIT_DEPTH_12) {
+     *x = twelveBitToSigned(&buf[0]);
+     *y = twelveBitToSigned(&buf[2]);
+     *z = twelveBitToSigned(&buf[4]);
+   } else {
+     *x = eightBitToSigned(&buf[0]);
+     *y = eightBitToSigned(&buf[1]);
+     *z = eightBitToSigned(&buf[2]);
+   }
+   
+   return 0;
+}
+
+int MMA8452::readXCount(int *x) {
+   char buf[2];
+   if(readXRaw((char*)&buf)) {
+       return 1;
+   }
+   if(_bitDepth==BIT_DEPTH_12) {
+     *x = twelveBitToSigned(&buf[0]);
+   } else {
+     *x = eightBitToSigned(&buf[0]);
+   }
+   return 0;
+}
+
+int MMA8452::readYCount(int *y) {
+   char buf[2];
+   if(readYRaw((char*)&buf)) {
+       return 1;
+   }
+   if(_bitDepth==BIT_DEPTH_12) {
+     *y = twelveBitToSigned(&buf[0]);
+   } else {
+     *y = eightBitToSigned(&buf[0]);
+   }
+   return 0;
+}
+
+int MMA8452::readZCount(int *z) {
+   char buf[2];
+   if(readZRaw((char*)&buf)) {
+       return 1;
+   }
+   if(_bitDepth==BIT_DEPTH_12) {
+     *z = twelveBitToSigned(&buf[0]);
+   } else {
+     *z = eightBitToSigned(&buf[0]);
+   }
+   return 0;
+}
+
+double MMA8452::convertCountToGravity(int count, int countsPerG) {
+   return (double)count/(double)countsPerG;
+}
+
+int MMA8452::getCountsPerG() {
+ // assume starting with DYNAMIC_RANGE_2G and BIT_DEPTH_12
+   int countsPerG = 1024;
+   if(_bitDepth==BIT_DEPTH_8) {
+      countsPerG = 64;
+   }
+   switch(_dynamicRange) {
+      case DYNAMIC_RANGE_4G:
+         countsPerG /= 2;
+      break;
+      case DYNAMIC_RANGE_8G:
+         countsPerG /= 4;
+      break;
+   }
+   return countsPerG;
+}
+
+int MMA8452::readXYZGravity(double *x, double *y, double *z) {
+   int xCount = 0, yCount = 0, zCount = 0;
+   if(readXYZCounts(&xCount,&yCount,&zCount)) {
+      return 1;
+   }
+   int countsPerG = getCountsPerG();
+   
+   *x = convertCountToGravity(xCount,countsPerG);
+   *y = convertCountToGravity(yCount,countsPerG);
+   *z = convertCountToGravity(zCount,countsPerG);
+   return 0;
+}
+
+int MMA8452::readXGravity(double *x) {
+   int xCount = 0;
+   if(readXCount(&xCount)) {
+      return 1;
+   }
+   int countsPerG = getCountsPerG();
+   
+   *x = convertCountToGravity(xCount,countsPerG);
+   return 0;
+}
+
+int MMA8452::readYGravity(double *y) {
+   int yCount = 0;
+   if(readYCount(&yCount)) {
+      return 1;
+   }
+   int countsPerG = getCountsPerG();
+   
+   *y = convertCountToGravity(yCount,countsPerG);
+   return 0;
+}
+
+int MMA8452::readZGravity(double *z) {
+   int zCount = 0;
+   if(readZCount(&zCount)) {
+      return 1;
+   }
+   int countsPerG = getCountsPerG();
+   
+   *z = convertCountToGravity(zCount,countsPerG);
+   return 0;
+}
+
+// apply an AND mask to a register. read register value, apply mask, write it back
+int MMA8452::logicalANDRegister(char addr, char mask) {
+   char value = 0;
+   // read register value
+   if(readRegister(addr,&value)) {
+      return 0;
+   }
+   // apply mask
+   value &= mask;
+   return writeRegister(addr,value);
+}
+
+
+// apply an OR mask to a register. read register value, apply mask, write it back
+int MMA8452::logicalORRegister(char addr, char mask) {
+   char value = 0;
+   // read register value
+   if(readRegister(addr,&value)) {
+      return 0;
+   }
+   // apply mask
+   value |= mask;
+   return writeRegister(addr,value);
+}
+
+// apply an OR mask to a register. read register value, apply mask, write it back
+int MMA8452::logicalXORRegister(char addr, char mask) {
+   char value = 0;
+   // read register value
+   if(readRegister(addr,&value)) {
+      return 0;
+   }
+   // apply mask
+   value ^= mask;
+   return writeRegister(addr,value);
+}
+
+// Write register (The device must be placed in Standby Mode to change the value of the registers) 
+int MMA8452::writeRegister(char addr, char data) {
+    // what this actually does is the following
+    // 1. tell I2C bus to start transaction
+    // 2. tell slave we want to write (slave address & write flag)
+    // 3. send the write address
+    // 4. send the data to write
+    // 5. tell I2C bus to end transaction
+
+    // we can wrap this up in the I2C library write function
+    char buf[2] = {0,0};
+    buf[0] = addr;
+    buf[1] = data;
+    return _i2c.write(MMA8452_ADDRESS, buf,2);
+    // note, could also do return writeRegister(addr,&data,1);
+}
+
+int MMA8452::eightBitToSigned(char *buf) {
+   return (int8_t)*buf;
+}
+
+int MMA8452::twelveBitToSigned(char *buf) {
+   // cheat by using the int16_t internal type
+   // all we need to do is convert to little-endian format and shift right
+   int16_t x = 0;
+   ((char*)&x)[1] = buf[0];
+   ((char*)&x)[0] = buf[1];
+   // note this only works because the below is an arithmetic right shift
+   return x>>4; 
+}
+
+int MMA8452::writeRegister(char addr, char *data, int nbytes) {
+    // writing multiple bytes is a little bit annoying because
+    // the I2C library doesn't support sending the address separately
+    // so we just do it manually
+    
+    // 1. tell I2C bus to start transaction
+    _i2c.start();
+    // 2. tell slave we want to write (slave address & write flag)
+    if(_i2c.write(_writeAddress)!=1) {
+       return 1;
+    }
+    // 3. send the write address
+    if(_i2c.write(addr)!=1) {
+       return 1;
+    }
+    // 4. send the data to write
+    for(int i=0; i<nbytes; i++) {
+       if(_i2c.write(data[i])!=1) {
+          return 1;
+       }
+    }
+    // 5. tell I2C bus to end transaction
+    _i2c.stop();
+    return 0;
+}
+
+int MMA8452::readRegister(char addr, char *dst, int nbytes) {
+    // this is a bit odd, but basically proceeds like this
+    // 1. Send a start command
+    // 2. Tell the slave we want to write (slave address & write flag)
+    // 3. Send the address of the register (addr)
+    // 4. Send another start command to delineate read portion
+    // 5. Tell the slave we want to read (slave address & read flag)
+    // 6. Read the register value bytes
+    // 7. Send a stop command
+    
+    // we can wrap this process in the I2C library read and write commands
+    if(_i2c.write(MMA8452_ADDRESS,&addr,1,true)) {
+       return 1;
+    }
+    return _i2c.read(MMA8452_ADDRESS,dst,nbytes);
+}
+
+// most registers are 1 byte, so here is a convenience function
+int MMA8452::readRegister(char addr, char *dst) {
+    return readRegister(addr,dst,1);
+}
+
+MMA8452::BitDepth MMA8452::getBitDepth() {
+   return _bitDepth;
+}
+
+#ifdef MMA8452_DEBUG
+void MMA8452::debugRegister(char reg) {
+   // get register value
+   char v = 0;
+   if(readRegister(reg,&v)) {
+      MMA8452_DBG("Error reading specified register");
+      return;
+   }
+   // print out details
+   switch(reg) {
+      case MMA8452_CTRL_REG_1:
+         MMA8452_DBG("CTRL_REG_1 has value: 0x%x",v);
+         MMA8452_DBG(" 7  ALSP_RATE_1: %d",(v&0x80)>>7);
+         MMA8452_DBG(" 6  ALSP_RATE_0: %d",(v&0x40)>>6);
+         MMA8452_DBG(" 5  DR2: %d",        (v&0x20)>>5);
+         MMA8452_DBG(" 4  DR1: %d",        (v&0x10)>>4);
+         MMA8452_DBG(" 3  DR0: %d",        (v&0x08)>>3);
+         MMA8452_DBG(" 2  LNOISE: %d",     (v&0x04)>>2);
+         MMA8452_DBG(" 1  FREAD: %d",      (v&0x02)>>1);
+         MMA8452_DBG(" 0  ACTIVE: %d",     (v&0x01));
+      break;
+        
+      case MMA8452_XYZ_DATA_CFG:
+         MMA8452_DBG("XYZ_DATA_CFG has value: 0x%x",v);
+         MMA8452_DBG(" 7  Unused: %d", (v&0x80)>>7);
+         MMA8452_DBG(" 6  0: %d",      (v&0x40)>>6);
+         MMA8452_DBG(" 5  0: %d",      (v&0x20)>>5);
+         MMA8452_DBG(" 4  HPF_Out: %d",(v&0x10)>>4);
+         MMA8452_DBG(" 3  0: %d",      (v&0x08)>>3);
+         MMA8452_DBG(" 2  0: %d",      (v&0x04)>>2);
+         MMA8452_DBG(" 1  FS1: %d",    (v&0x02)>>1);
+         MMA8452_DBG(" 0  FS0: %d",    (v&0x01));
+         switch(v&0x03) {
+            case 0:
+               MMA8452_DBG("Dynamic range: 2G");
+            break;
+            case 1:
+               MMA8452_DBG("Dynamic range: 4G");
+            break;
+            case 2:
+               MMA8452_DBG("Dynamic range: 8G");
+            break;
+            default:
+               MMA8452_DBG("Unknown dynamic range");
+            break;
+         }
+      break;
+      
+      case MMA8452_STATUS:
+         MMA8452_DBG("STATUS has value: 0x%x",v);
+         MMA8452_DBG(" 7  ZYXOW: %d",(v&0x80)>>7);
+         MMA8452_DBG(" 6  ZOW: %d",  (v&0x40)>>6);
+         MMA8452_DBG(" 5  YOW: %d",  (v&0x20)>>5);
+         MMA8452_DBG(" 4  XOW: %d",  (v&0x10)>>4);
+         MMA8452_DBG(" 3  ZYXDR: %d",(v&0x08)>>3);
+         MMA8452_DBG(" 2  ZDR: %d",  (v&0x04)>>2);
+         MMA8452_DBG(" 1  YDR: %d",  (v&0x02)>>1);
+         MMA8452_DBG(" 0  XDR: %d",  (v&0x01));
+      break;
+      
+      default:
+         MMA8452_DBG("Unknown register address: 0x%x",reg);
+      break;
+   }
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8452.h	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,343 @@
+#pragma once
+
+// Authors: Ashley Mills, Nicholas Herriot
+/* Copyright (c) 2013 Vodafone, 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.
+ */
+
+// the SparkFun breakout board defaults to 1, set to 0 if SA0 jumper on the bottom of the board is set
+// see the Table 10. I2C Device Address Sequence in Freescale MMA8452Q pdf
+ 
+#include "mbed.h" 
+
+#define MMA8452_DEBUG 1
+
+// More info on MCU Master address can be found on section 5.10.1 of http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MMA8452Q
+#define SA0 1
+#if SA0
+  #define MMA8452_ADDRESS 0x3A // 0x1D<<1  // SA0 is high, 0x1C if low - 
+#else
+  #define MMA8452_ADDRESS 0x38 // 0x1C<<1
+#endif
+
+// Register descriptions found in section 6 of pdf
+#define MMA8452_STATUS 0x00        // Type 'read' : Status of the data registers
+#define MMA8452_OUT_X_MSB 0x01     // Type 'read' : x axis - MSB of 2 byte sample
+#define MMA8452_OUT_X_LSB 0x02     // Type 'read' : x axis - LSB of 2 byte sample
+#define MMA8452_OUT_Y_MSB 0x03     // Type 'read' : y axis - MSB of 2 byte sample
+#define MMA8452_OUT_Y_LSB 0x04     // Type 'read' : y axis - LSB of 2 byte sample
+#define MMA8452_OUT_Z_MSB 0x05     // Type 'read' : z axis - MSB of 2 byte sample
+#define MMA8452_OUT_Z_LSB 0x06     // Type 'read' : z axis - LSB of 2 byte sample
+
+// register definitions
+#define MMA8452_XYZ_DATA_CFG 0x0E
+
+#define MMA8452_SYSMOD 0x0B        // Type 'read' : This tells you if device is active, sleep or standy 0x00=STANDBY 0x01=WAKE 0x02=SLEEP
+#define MMA8452_WHO_AM_I 0x0D      // Type 'read' : This should return the device id of 0x2A
+
+#define MMA8452_PL_STATUS 0x10     // Type 'read' : This shows portrait landscape mode orientation
+#define MMA8452_PL_CFG 0x11        // Type 'read/write' : This allows portrait landscape configuration
+#define MMA8452_PL_COUNT 0x12      // Type 'read' : This is the portraint landscape debounce counter
+#define MMA8452_PL_BF_ZCOMP 0x13   // Type 'read' :
+#define MMA8452_PL_THS_REG 0x14    // Type 'read' :
+
+#define MMA8452_FF_MT_CFG 0X15     // Type 'read/write' : Freefaul motion functional block configuration
+#define MMA8452_FF_MT_SRC 0X16     // Type 'read' : Freefaul motion event source register
+#define MMA8452_FF_MT_THS 0X17     // Type 'read' : Freefaul motion threshold register
+#define MMA8452_FF_COUNT 0X18      // Type 'read' : Freefaul motion debouce counter
+
+#define MMA8452_ASLP_COUNT 0x29    // Type 'read/write' : Counter settings for auto sleep
+#define MMA8452_CTRL_REG_1 0x2A    // Type 'read/write' :
+#define MMA8452_CTRL_REG_2 0x2B    // Type 'read/write' :
+#define MMA8452_CTRL_REG_3 0x2C    // Type 'read/write' :
+#define MMA8452_CTRL_REG_4 0x2D    // Type 'read/write' :
+#define MMA8452_CTRL_REG_5 0x2E    // Type 'read/write' :
+
+// Defined in table 13 of the Freescale PDF
+/// xxx these all need to have better names
+#define STANDBY 0x00                        // State value returned after a SYSMOD request, it can be in state STANDBY, WAKE or SLEEP
+#define WAKE 0x01                           // State value returned after a SYSMOD request, it can be in state STANDBY, WAKE or SLEEP
+#define SLEEP 0x02                          // State value returned after a SYSMOD request, it can be in state STANDBY, WAKE or SLEEP
+#define ACTIVE 0x01                         // Stage value returned and set in Control Register 1, it can be STANDBY=00, or ACTIVE=01
+ 
+#define TILT_STATUS 0x03        // Tilt Status (Read only)
+#define SRST_STATUS 0x04        // Sample Rate Status Register (Read only)
+#define SPCNT_STATUS 0x05       // Sleep Count Register (Read/Write)
+#define INTSU_STATUS 0x06       // Interrupt Setup Register
+#define MODE_STATUS 0x07        // Mode Register (Read/Write)
+#define SR_STATUS 0x08          // Auto-Wake and Active Mode Portrait/Landscape Samples per Seconds Register (Read/Write)
+#define PDET_STATUS 0x09        // Tap/Pulse Detection Register (Read/Write)
+#define PD_STATUS 0xA           // Tap/Pulse Debounce Count Register (Read/Write)
+ 
+// masks for enabling/disabling standby
+#define MMA8452_ACTIVE_MASK 0x01
+#define MMA8452_STANDBY_MASK 0xFE
+
+// mask for dynamic range reading and writing
+#define MMA8452_DYNAMIC_RANGE_MASK 0xFC
+
+// mask and shift for data rate reading and writing
+#define MMA8452_DATA_RATE_MASK 0xC7
+#define MMA8452_DATA_RATE_MASK_SHIFT 0x03
+
+// mask and shift for general reading and writing
+#define MMA8452_WRITE_MASK 0xFE
+#define MMA8452_READ_MASK 0x01
+
+// mask and shift for bit depth reading and writing
+#define MMA8452_BIT_DEPTH_MASK 0xFD
+#define MMA8452_BIT_DEPTH_MASK_SHIFT 0x01
+
+// status masks and shifts
+#define MMA8452_STATUS_ZYXDR_MASK 0x08
+#define MMA8452_STATUS_ZDR_MASK 0x04
+#define MMA8452_STATUS_YDR_MASK 0x02
+#define MMA8452_STATUS_XDR_MASK 0x01
+
+/**
+ * Wrapper for the MMA8452 I2C driven accelerometer.
+ */
+class MMA8452 {
+            
+    public:
+    
+       enum DynamicRange {
+           DYNAMIC_RANGE_2G=0x00,
+           DYNAMIC_RANGE_4G,
+           DYNAMIC_RANGE_8G,
+           DYNAMIC_RANGE_UNKNOWN
+       };
+        
+       enum BitDepth {
+           BIT_DEPTH_12=0x00,
+           BIT_DEPTH_8, // 1 sets fast read mode, hence the inversion
+           BIT_DEPTH_UNKNOWN
+       };
+       
+       enum DataRateHz {
+          RATE_800=0x00,
+          RATE_400,
+          RATE_200,
+          RATE_100,
+          RATE_50,
+          RATE_12_5,
+          RATE_6_25,
+          RATE_1_563,
+          RATE_UNKNOWN
+       };
+         
+       /**
+        * Create an accelerometer object connected to the specified I2C pins.
+        *
+        * @param sda I2C data port
+        * @param scl I2C clock port
+        * @param frequency 
+        * 
+        */ 
+      MMA8452(PinName sda, PinName scl, int frequency);
+       
+      /// Destructor
+      ~MMA8452();
+      
+      /**
+       * Puts the MMA8452 in active mode.
+       * @return 0 on success, 1 on failure.
+       */
+      int activate();
+ 
+      /**
+       * Puts the MMA8452 in standby.
+       * @return 0 on success, 1 on failure.
+       */
+      int standby();      
+    
+       /**
+        * Read the device ID from the accelerometer (should be 0x2a)
+        *
+        * @param dst pointer to store the ID
+        * @return 0 on success, 1 on failure.
+        */        
+      int getDeviceID(char* dst);  
+      
+      /**
+       * Read the MMA8452 status register.
+       *
+       * @param dst pointer to store the register value.
+       * @ return 0 on success, 1 on failure.
+       */
+      int getStatus(char* dst);  
+      
+      /** 
+       * Read the raw x, y, an z registers of the MMA8452 in one operation.
+       * All three registers are read sequentially and stored in the provided buffer.
+       * The stored values are signed 2's complement left-aligned 12 or 8 bit integers.
+       *
+       * @param dst The destination buffer. Note that this needs to be 3 bytes for
+       * BIT_DEPTH_8 and 6 bytes for BIT_DEPTH_12. It is upto the caller to ensure this.
+       * @return 0 for success, and 1 for failure
+       * @sa setBitDepth
+       */ 
+      int readXYZRaw(char *dst);
+      
+      /// Read the raw x register into the provided buffer. @sa readXYZRaw
+      int readXRaw(char *dst);
+      /// Read the raw y register into the provided buffer. @sa readXYZRaw
+      int readYRaw(char *dst);
+      /// Read the raw z register into the provided buffer. @sa readXYZRaw
+      int readZRaw(char *dst);
+      
+      /**
+       * Read the x, y, and z signed counts of the MMA8452 axes.
+       * 
+       * Count resolution is either 8 bits or 12 bits, and the range is either +-2G, +-4G, or +-8G
+       * depending on settings. The number of counts per G are 1024, 512, 256 for 2,4, and 8 G
+       * respectively at 12 bit resolution and 64, 32, 16 for 2, 4, and 8 G respectively at
+       * 8 bit resolution.
+       * 
+       * This function queries the MMA8452 and returns the signed counts for each axes.
+       *
+       * @param x Pointer to integer to store x count
+       * @param y Pointer to integer to store y count
+       * @param z Pointer to integer to store z count
+       * @return 0 on success, 1 on failure.
+       */
+      int readXYZCounts(int *x, int *y, int *z);
+      
+      /// Read the x axes signed count. @sa readXYZCounts
+      int readXCount(int *x);
+      /// Read the y axes signed count. @sa readXYZCounts
+      int readYCount(int *y);
+      /// Read the z axes signed count. @sa readXYZCounts
+      int readZCount(int *z);
+      
+      /**
+       * Read the x, y, and z accelerations measured in G.
+       *
+       * The measurement resolution is controlled via setBitDepth which can
+       * be 8 or 12, and by setDynamicRange, which can be +-2G, +-4G, or +-8G.
+       *
+       * @param x A pointer to the double to store the x acceleration in.
+       * @param y A pointer to the double to store the y acceleration in.
+       * @param z A pointer to the double to store the z acceleration in.
+       *
+       * @return 0 on success, 1 on failure.
+       */
+      int readXYZGravity(double *x, double *y, double *z);
+      
+      /// Read the x gravity in G into the provided double pointer. @sa readXYZGravity
+      int readXGravity(double *x);
+      /// Read the y gravity in G into the provided double pointer. @sa readXYZGravity
+      int readYGravity(double *y);
+      /// Read the z gravity in G into the provided double pointer. @sa readXYZGravity
+      int readZGravity(double *z);
+      
+      /// Returns 1 if data has been internally sampled (is available) for all axes since last read, 0 otherwise.
+      int isXYZReady();
+      /// Returns 1 if data has been internally sampled (is available) for the x-axis since last read, 0 otherwise.
+      int isXReady();
+      /// Returns 1 if data has been internally sampled (is available) for the y-axis since last read, 0 otherwise.
+      int isYReady();
+      /// Returns 1 if data has been internally sampled (is available) for the z-axis since last read, 0 otherwise.
+      int isZReady();
+            
+      /** 
+       * Reads a single byte from the specified MMA8452 register.
+       *
+       * @param addr The internal register address.
+       * @param dst The destination buffer address.
+       * @return 1 on success, 0 on failure.
+       */
+      int readRegister(char addr, char *dst);
+      
+      /** 
+       * Reads n bytes from the specified MMA8452 register.
+       *
+       * @param addr The internal register address.
+       * @param dst The destination buffer address.
+       * @param nbytes The number of bytes to read.
+       * @return 1 on success, 0 on failure.
+       */
+      int readRegister(char addr, char *dst, int nbytes);
+        
+       /** 
+        * Write to the specified MMA8452 register.
+        *
+        * @param addr The internal register address
+        * @param data Data byte to write
+        */    
+      int writeRegister(char addr, char data);
+      
+       /** 
+        * Write a data buffer to the specified MMA8452 register.
+        *
+        * @param addr The internal register address
+        * @param data Pointer to data buffer to write
+        * @param nbytes The length of the data buffer to write
+        */    
+      int writeRegister(char addr, char *data, int nbytes);
+      
+      int setDynamicRange(DynamicRange range, int toggleActivation=1);
+      int setBitDepth(BitDepth depth, int toggleActivation=1);
+      int setDataRate(DataRateHz dataRate, int toggleActivation=1);
+      
+      DynamicRange getDynamicRange();
+      DataRateHz getDataRate();
+      BitDepth getBitDepth();
+      
+      #ifdef MMA8452_DEBUG
+      void debugRegister(char reg);
+      #endif
+   
+    private:
+      /**
+       * Reads the specified register, applies the mask with logical AND, logical ORs the value
+       * and writes back the result to the register. If toggleActivation is set to true then the
+       * device is put in standby before the operation, and activated at the end.
+       * Setting it to false is useful for setting options on a device that you want to keep in
+       * standby.
+       */
+      int maskAndApplyRegister(char reg, char mask, char value, int toggleActivation);
+      
+      /// Reads the specified register, applies the mask with logical AND, and writes the result back.
+      int logicalANDRegister(char addr, char mask);
+      /// Reads the specified register, applies the mask with logical OR, and writes the result back.
+      int logicalORRegister(char addr, char mask);
+      /// Reads the specified register, applies the mask with logical XOR, and writes the result back.
+      int logicalXORRegister(char addr, char mask);
+      
+      /// Converts the 12-bit two's complement number in buf to a signed integer. Returns the integer.
+      int twelveBitToSigned(char *buf);
+      /// Converts the 8-bit two's complement number in buf to a signed integer. Returns the integer.
+      int eightBitToSigned(char *buf);
+      
+      /// Converts a count to a gravity using the supplied countsPerG. Returns the gravity.
+      double convertCountToGravity(int count, int countsPerG);
+      
+      /// Reads the register at addr, applies the mask with logical AND, and returns the result.
+      char getMaskedRegister(int addr, char mask);
+      
+      /// Get the counts per G for the current settings of bit depth and dynamic range.
+      int getCountsPerG();
+    
+      I2C _i2c;
+      int _frequency;
+      int _readAddress;
+      int _writeAddress;
+      
+      BitDepth _bitDepth;
+      DynamicRange _dynamicRange;       
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PinDetect.lib	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.cpp	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,74 @@
+/* mbed R/C Servo Library
+ *  
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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 "Servo.h"
+#include "mbed.h"
+
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+Servo::Servo(PinName pin) : _pwm(pin) {
+    calibrate();
+    write(0.5);
+}
+
+void Servo::write(float percent) {
+    float offset = _range * 2.0 * (percent - 0.5);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+    _p = clamp(percent, 0.0, 1.0);
+}
+
+void Servo::position(float degrees) {
+    float offset = _range * (degrees / _degrees);
+    _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
+}
+
+void Servo::calibrate(float range, float degrees) {
+    _range = range;
+    _degrees = degrees;
+}
+
+float Servo::read() {
+    return _p;
+}
+
+Servo& Servo::operator= (float percent) { 
+    write(percent);
+    return *this;
+}
+
+Servo& Servo::operator= (Servo& rhs) {
+    write(rhs.read());
+    return *this;
+}
+
+Servo::operator float() {
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.h	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,98 @@
+/* mbed R/C Servo Library
+ * Copyright (c) 2007-2010 sford, cstyles
+ *
+ * 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 MBED_SERVO_H
+#define MBED_SERVO_H
+
+#include "mbed.h"
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * // Continuously sweep the servo through it's full range
+ * #include "mbed.h"
+ * #include "Servo.h"
+ * 
+ * Servo myservo(p21);
+ * 
+ * int main() {
+ *     while(1) {
+ *         for(int i=0; i<100; i++) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *         for(int i=100; i>0; i--) {
+ *             myservo = i/100.0;
+ *             wait(0.01);
+ *         }
+ *     }
+ * }
+ * @endcode
+ */
+class Servo {
+
+public:
+    /** Create a servo object connected to the specified PwmOut pin
+     *
+     * @param pin PwmOut pin to connect to 
+     */
+    Servo(PinName pin);
+    
+    /** Set the servo position, normalised to it's full range
+     *
+     * @param percent A normalised number 0.0-1.0 to represent the full range.
+     */
+    void write(float percent);
+    
+    /**  Read the servo motors current position
+     *
+     * @param returns A normalised number 0.0-1.0  representing the full range.
+     */
+    float read();
+    
+    /** Set the servo position
+     *
+     * @param degrees Servo position in degrees
+     */
+    void position(float degrees);
+    
+    /**  Allows calibration of the range and angles for a particular servo
+     *
+     * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
+     * @param degrees Angle from centre to maximum/minimum position in degrees
+     */
+    void calibrate(float range = 0.0005, float degrees = 45.0); 
+        
+    /**  Shorthand for the write and read functions */
+    Servo& operator= (float percent);
+    Servo& operator= (Servo& rhs);
+    operator float();
+
+protected:
+    PwmOut _pwm;
+    float _range;
+    float _degrees;
+    float _p;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,9 @@
+// Geometry / configuration - included from the IK library
+
+// See Geometry.md for a description of these measurements
+
+// You can work in any units, as long as they all match; these
+// will dictate the units of the 3D space you're in.  I used millimetres.
+float L1=80; //Shoulder to elbow length
+float L2=80; //Elbow to wrise length
+float L3=68;  //Length from wrist to hand PLUS base centre to shoulder
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ik.cpp	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,97 @@
+/* Inverse kinetics, Nick Moriarty May 2014
+This code is provided under the terms of the MIT license.
+The MIT License (MIT)
+Copyright (c) 2014 Nick Moriarty
+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 "math.h"
+#include "ik.h"
+#include "config.h"
+
+const float PI=3.14159265359;
+
+// Get polar coords from cartesian ones
+void cart2polar(float a, float b, float& r, float& theta)
+{
+    // Determine magnitude of cartesian coords
+    r = sqrt(a*a + b*b);
+
+    // Don't try to calculate zero-magnitude vectors' angles
+    if(r == 0) return;
+
+    float c = a / r;
+    float s = b / r;
+
+    // Safety!
+    if(s > 1) s = 1;
+    if(c > 1) c = 1;
+    if(s < -1) s = -1;
+    if(c < -1) c = -1;
+
+    // Calculate angle in 0..PI
+    theta = acos(c);
+
+    // Convert to full range
+    if(s < 0) theta *= -1;
+}
+
+// Get angle from a triangle using cosine rule
+bool cosangle(float opp, float adj1, float adj2, float& theta)
+{
+    // Cosine rule:
+    // C^2 = A^2 + B^2 - 2*A*B*cos(angle_AB)
+    // cos(angle_AB) = (A^2 + B^2 - C^2)/(2*A*B)
+    // C is opposite
+    // A, B are adjacent
+    float den = 2*adj1*adj2;
+
+    if(den==0) return false;
+    float c = (adj1*adj1 + adj2*adj2 - opp*opp)/den;
+
+    if(c>1 || c<-1) return false;
+
+    theta = acos(c);
+
+    return true;
+}
+
+// Solve angles!
+bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
+{
+    // Solve top-down view
+    float r, th0;
+    cart2polar(y, x, r, th0);
+
+    // Account for the wrist length!
+    r -= L3;
+
+    // In arm plane, convert to polar
+    float ang_P, R;
+    cart2polar(r, z, R, ang_P);
+
+    // Solve arm inner angles as required
+    float B, C;
+    if(!cosangle(L2, L1, R, B)) return false;
+    if(!cosangle(R, L1, L2, C)) return false;
+
+    // Solve for servo angles from horizontal
+    a0 = th0;
+    a1 = ang_P + B;
+    a2 = C + a1 - PI;
+
+    return true;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ik.h	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,19 @@
+/* meArmIK - York Hackspace May 2014
+ * Inverse Kinematics solver for three degrees of freedom
+ * created for Phenoptix' meArm robot arm
+ */
+#ifndef IK_H_INCLUDED
+#define IK_H_INCLUDED
+
+extern float L1, L2, L3;
+
+// Get polar coords from cartesian ones
+void cart2polar(float a, float b, float& r, float& theta);
+
+// Get angle from a triangle using cosine rule
+bool cosangle(float opp, float adj1, float adj2, float& theta);
+
+// Solve angles!
+bool solve(float x, float y, float z, float& a0, float& a1, float& a2);
+
+#endif // IK_H_INCLUDED
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,119 @@
+#include "meArm.h"
+#include "Servo.h"
+#include "MMA8452.h"
+#include "mbed.h"
+#include "PinDetect.h"
+
+meArm arm;
+
+Serial pc(USBTX,USBRX);
+
+DigitalOut myLed(LED1);
+PinDetect pb1(p20);
+
+MMA8452 mma(p28, p27, 100000);
+int x, y, z;
+ 
+//Change these values if accelerometer reading are different:
+//How far the accerometer is tilted before starting to move the arm:
+const int MovementThreshold = 18;
+ 
+//The average zero acceleration values read from the accelerometer for each axis:
+const int ZeroXValue = 0;
+const int ZeroYValue = 0;
+const int ZeroZValue = 0;
+ 
+//The maximum (positive) acceleration values read from the accelerometer for each axis:
+const int MaxXValue = 4096;
+const int MaxYValue = 4096;
+const int MaxZValue = 4096;
+ 
+//The minimum (negative) acceleration values read from the accelerometer for each axis:
+const int MinXValue = -4096;
+const int MinYValue = -4096;
+const int MinZValue = -4096;
+ 
+//The sign of the arm movement relative to the acceleration.
+//If arm is going in the opposite direction you think it should go, change the sign for the appropriate axis.
+const int XSign = 1;
+const int YSign = 1;
+const int ZSign = 1;
+ 
+//The maximum speed in each axis (x and y)
+//that the arm should move. Set this to a higher or lower number if the arm does not move fast enough or is too fast.
+const int MaxArmMovement = 50;  
+ 
+//This reduces the 'twitchiness' of the cursor by calling a delay function at the end of the main loop.
+//There are better way to do this without delaying the whole microcontroller, but that is left for another tutorial or project.
+const int ArmDelay = .001;
+
+//Function to process the acclerometer data
+//and send mouse movement information via USB
+void processAccelerometer(int16_t XReading, int16_t YReading, int16_t ZReading)
+{
+  //Initialize values for the mouse cursor movement.
+  int16_t ArmXMovement = 0;
+  int16_t ArmYMovement = 0;
+  int16_t ArmZMovement = 0;
+  
+  //Calculate mouse movement
+  //If the analog X reading is ouside of the zero threshold...
+  if( MovementThreshold < abs( XReading - ZeroXValue ) ){
+    //...calculate X mouse movement based on how far the X acceleration is from its zero value.
+    ArmXMovement = XSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxXValue - MinXValue ) ) * ( XReading - MinXValue ) ) - MaxArmMovement );
+    //it could use some improvement, like making it trigonometric.
+  }  else  {
+    //Within the zero threshold, the cursor does not move in the X.
+    ArmXMovement = 0;
+  }
+ 
+  //If the analog Y reading is ouside of the zero threshold... 
+  if( MovementThreshold < abs( YReading - ZeroYValue ) ){
+    //...calculate Y mouse movement based on how far the Y acceleration is from its zero value.
+    ArmYMovement = YSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxYValue - MinYValue ) ) * ( YReading - MinYValue ) ) - MaxArmMovement );
+    //it could use some improvement, like making it trigonometric.
+  }  else  {
+    //Within the zero threshold, the cursor does not move in the Y.
+    ArmYMovement = 0;
+  }
+
+    //Calculate mouse movement
+  //If the analog Z reading is ouside of the zero threshold...
+  if( MovementThreshold < abs( ZReading - ZeroZValue ) )
+  {
+    //...calculate Z mouse movement based on how far the Z acceleration is from its zero value.
+    ArmZMovement = ZSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxZValue - MinZValue ) ) * ( ZReading - MinZValue ) ) - MaxArmMovement );
+    //it could use some improvement, like making it trigonometric.
+  }
+  else
+  {
+    //Within the zero threshold, the cursor does not move in the X.
+    ArmZMovement = 0;
+  }
+  arm.gotoPoint(ArmXMovement, ArmYMovement, ArmZMovement);  // otherwise just move mouse
+}
+
+void swing(){
+    arm.openGripper();
+    arm.closeGripper();
+  }
+  
+int main() { 
+  pb1.mode(PullUp);
+  mma.readXYZCounts(&x, &y, &z);    // get an initial read   
+  arm.begin();               // Initialize arm
+
+  while(1) {
+    
+    if(!pb1){
+        myLed=1;
+        swing();
+        myLed=0;
+    }
+    
+    mma.readXYZCounts(&x, &y, &z);  //   // Read the 'raw' data in 14-bit counts
+    printf("\n(%.2d,%.2d,%.2d)", x,y,z);   
+    processAccelerometer(x,y,z);  // Work with the read data    
+    wait(ArmDelay);  // wait until next reading - was 500 in Adafruit example
+  }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/meArm.cpp	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,154 @@
+/* meArm library York Hack Space May 2014
+ * A simple control library for Phenoptix' meArm
+ * Usage:
+ *   meArm arm;
+ *   arm.begin(1, 10, 9, 6);
+ *   arm.openGripper();
+ *   arm.gotoPoint(-80, 100, 140);
+ *   arm.closeGripper();
+ *   arm.gotoPoint(70, 200, 10);
+ *   arm.openGripper();
+ */
+ 
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+
+#include "mbed.h"
+#include "ik.h"
+#include "meArm.h"
+#include "Servo.h"
+
+Servo _base(p21);
+Servo _shoulder(p22);
+Servo _elbow(p23);
+Servo _gripper(p24);
+
+bool setup_servo (ServoInfo& svo, const int n_min, const int n_max,
+                  const float a_min, const float a_max)
+{
+    float n_range = n_max - n_min;
+    float a_range = a_max - a_min;
+
+    // Must have a non-zero angle range
+    if(a_range == 0) return false;
+
+    // Calculate gain and zero
+    svo.gain = n_range / a_range;
+    svo.zero = n_min - svo.gain * a_min;
+
+    // Set limits
+    svo.n_min = n_min;
+    svo.n_max = n_max;
+
+    return true;
+}
+
+int angle2pwm (const ServoInfo& svo, const float angle)
+{
+    float pwm = 0.5f + svo.zero + svo.gain * angle;
+    return int(pwm);
+}
+
+//Full constructor with calibration data
+meArm::meArm(int sweepMinBase, int sweepMaxBase, float angleMinBase, float angleMaxBase,
+             int sweepMinShoulder, int sweepMaxShoulder, float angleMinShoulder, float angleMaxShoulder,
+             int sweepMinElbow, int sweepMaxElbow, float angleMinElbow, float angleMaxElbow,
+             int sweepMinGripper, int sweepMaxGripper, float angleMinGripper, float angleMaxGripper)
+{
+    setup_servo(_svoBase, sweepMinBase, sweepMaxBase, angleMinBase, angleMaxBase);
+    setup_servo(_svoShoulder, sweepMinShoulder, sweepMaxShoulder, angleMinShoulder, angleMaxShoulder);
+    setup_servo(_svoElbow, sweepMinElbow, sweepMaxElbow, angleMinElbow, angleMaxElbow);
+    setup_servo(_svoGripper, sweepMinGripper, sweepMaxGripper, angleMinGripper, angleMaxGripper);
+}
+
+void meArm::begin() {  
+  printf("Begun!");
+  goDirectlyTo(0, 100, 50);
+}
+
+
+//Set servos to reach a certain point directly without caring how we get there 
+void meArm::goDirectlyTo(float x, float y, float z) {
+  float radBase,radShoulder,radElbow;
+  if (solve(x, y, z, radBase, radShoulder, radElbow)) {
+    _base.position(angle2pwm(_svoBase,toDegrees(radBase))/100);
+    _shoulder.position(angle2pwm(_svoShoulder,toDegrees(radShoulder))/100);
+    _elbow.position(angle2pwm(_svoElbow,toDegrees(radElbow))/100);
+    _x = x; _y = y; _z = z;
+  }    
+}
+
+//Travel smoothly from current point to another point
+void meArm::gotoPoint(float x, float y, float z) {
+  //Starting points - current pos
+  float x0 = _x; 
+  float y0 = _y; 
+  float z0 = _z;
+  float dist = sqrt((x0-x)*(x0-x)+(y0-y)*(y0-y)+(z0-z)*(z0-z));
+  int step = 5;
+  for (int i = 0; i<dist; i+= step) {
+    goDirectlyTo(x0 + (x-x0)*i/dist, y0 + (y-y0) * i/dist, z0 + (z-z0) * i/dist);
+  }
+  goDirectlyTo(x, y, z);
+  wait(1);
+}
+
+//Get x and y from theta and r
+void meArm::polarToCartesian(float theta, float r, float& x, float& y){
+    _r = r;
+    _t = theta;
+    x = r*sin(theta);
+    y = r*cos(theta);
+}
+
+//Same as above but for cylindrical polar coodrinates
+void meArm::gotoPointCylinder(float theta, float r, float z){
+    float x, y;
+    polarToCartesian(theta, r, x, y);
+    gotoPoint(x,y,z);
+}
+
+void meArm::goDirectlyToCylinder(float theta, float r, float z){
+    float x, y;
+    polarToCartesian(theta, r, x, y);
+    goDirectlyTo(x,y,z);
+}
+
+//Check to see if possible
+bool meArm::isReachable(float x, float y, float z) {
+  float radBase,radShoulder,radElbow;
+  return (solve(x, y, z, radBase, radShoulder, radElbow));
+}
+
+//Grab something
+void meArm::openGripper() {
+  _gripper.write(1.0);
+  wait(0.5);
+}
+
+//Let go of something
+void meArm::closeGripper() {
+  _gripper.write(0.0);
+  wait(0.5);
+}
+
+//Current x, y and z
+float meArm::getX() {
+  return _x;
+}
+float meArm::getY() {
+  return _y;
+}
+float meArm::getZ() {
+  return _z;
+}
+
+
+float meArm::getR() {
+  return _r;
+}
+float meArm::getTheta() {
+  return _t;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/meArm.h	Thu Dec 06 01:30:51 2018 +0000
@@ -0,0 +1,62 @@
+/* meArm library York Hack Space May 2014
+ * A simple control library for Phenoptix' meArm
+ * Usage:
+ *   meArm arm;
+ *   arm.begin(1, 10, 9, 6);
+ *   arm.openGripper();
+ *   arm.gotoPoint(-80, 100, 140);
+ *   arm.closeGripper();
+ *   arm.gotoPoint(70, 200, 10);
+ *   arm.openGripper();
+ */
+#ifndef MEARM_H
+#define MEARM_H
+
+const float pi=3.14159265359;
+
+struct ServoInfo {
+    int n_min, n_max;   // PWM 'soft' limits - should be just within range
+    float gain;         // PWM per radian
+    float zero;         // Theoretical PWM for zero angle
+};
+
+class meArm {
+  public:
+    //Full constructor uses calibration data, or can just give pins
+    meArm(int sweepMinBase=145, int sweepMaxBase=49, float angleMinBase=-pi/4, float angleMaxBase=pi/4,
+      int sweepMinShoulder=118, int sweepMaxShoulder=22, float angleMinShoulder=pi/4, float angleMaxShoulder=3*pi/4,
+      int sweepMinElbow=144, int sweepMaxElbow=36, float angleMinElbow=pi/4, float angleMaxElbow=-pi/4,
+      int sweepMinGripper=115, int sweepMaxGripper=115, float angleMinGripper=pi/4, float angleMaxGripper=pi/4);
+    //required before running
+    void begin();
+    //Travel smoothly from current point to another point
+    void gotoPoint(float x, float y, float z);
+    //Set servos to reach a certain point directly without caring how we get there 
+    void goDirectlyTo(float x, float y, float z);
+
+    //Same as above but for cylindrical polar coodrinates
+    void gotoPointCylinder(float theta, float r, float z);
+    void goDirectlyToCylinder(float theta, float r, float z);
+
+    //Grab something
+    void openGripper();
+    //Let go of something
+    void closeGripper();
+    //Check to see if possible
+    bool isReachable(float x, float y, float z);
+    //Current x, y and z
+    float getX();
+    float getY();
+    float getZ();
+
+    float getR();
+    float getTheta();
+  private:
+    void polarToCartesian(float theta, float r, float& x, float& y);
+    float _x, _y, _z;
+    float _r, _t;
+    ServoInfo _svoBase, _svoShoulder, _svoElbow, _svoGripper;
+    int _pinBase, _pinShoulder, _pinElbow, _pinGripper;
+};
+
+#endif
\ No newline at end of file