diff --git a/firmware/README.md b/firmware/README.md index 47fbcf4..9231ad4 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -1,4 +1,4 @@ Arduino Code ============ -In order to use this code, you need to put the AccelStepper v1.46 library in your libraries folder. Thanks to [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/). \ No newline at end of file +In order to use this code, you need to put the AccelStepper v1.46 library and SoftI2CMaster library in your libraries folder. Thanks to [AccelStepper library](http://www.airspayce.com/mikem/arduino/AccelStepper/) and [SoftI2CMaster](https://github.com/todbot/SoftI2CMaster). diff --git a/firmware/dc_motor_controller/dc_motor_controller.ino b/firmware/dc_motor_controller/dc_motor_controller.ino index 516bc61..9579378 100644 --- a/firmware/dc_motor_controller/dc_motor_controller.ino +++ b/firmware/dc_motor_controller/dc_motor_controller.ino @@ -1,25 +1,35 @@ +#include "SoftI2CMaster.h" +#include + /* H-bridge defines */ #define PWM1M1 5 #define PWM2M1 6 +#define PWM1M2 9 +#define PWM2M2 10 /* Limits for control signal */ -#define outMax 255 +#define outMax 127 #define outMin 0 /* Encoder defines */ -#define SELECT_PIN 7 -#define CLOCK_PIN 8 -#define DATA_PIN 9 +#define sda_pin 16 +#define scl_pin 14 +#define as5601_adr 0x36 +#define raw_ang_high 0x0c +#define raw_ang_low 0x0d +#define status_reg 0x0b /* Ratio of worm gear */ #define RATIO 30 /* Maximum Angle for homing scanning */ -#define MAX_AZ_ANGLE 370 +#define MAX_AZ_ANGLE 110 +#define MAX_EL_ANGLE 370 /* Homing switch */ -#define HOME_AZ 2 +#define HOME_AZ 4 +#define HOME_EL 7 /* Change to LOW according to Home sensor */ -#define DEFAULT_HOME_STATE HIGH +#define DEFAULT_HOME_STATE 1 /* Time to disable the motors in millisecond */ #define T_DELAY 60000 /* PIN for Enable or Disable Stepper Motors */ -#define EN 4 +#define EN 8 /* Serial configuration */ #define BufferSize 256 #define BaudRate 19200 @@ -27,42 +37,49 @@ unsigned long t_DIS = 0; /*time to disable the Motors*/ /* angle offset */ double az_angle_offset = 0; +double el_angle_offset = 0; +/* Encoder */ +SoftI2CMaster i2c_soft = SoftI2CMaster(scl_pin, sda_pin, 1); void setup() { /* H-bridge */ pinMode(OUTPUT, PWM1M1); pinMode(OUTPUT, PWM2M1); + pinMode(OUTPUT, PWM1M2); + pinMode(OUTPUT, PWM2M2); /* Encoder */ - pinMode(DATA_PIN, INPUT); - pinMode(CLOCK_PIN, OUTPUT); - pinMode(SELECT_PIN, OUTPUT); - digitalWrite(CLOCK_PIN, HIGH); - digitalWrite(SELECT_PIN, HIGH); + i2c_soft.begin(); + Wire.begin(); /* Homing switch */ pinMode(HOME_AZ, INPUT_PULLUP); + pinMode(HOME_EL, INPUT_PULLUP); /* Serial Communication */ Serial.begin(BaudRate); - /* Initial Homing */ - Homing(-MAX_AZ_ANGLE, true); /* Enable Motors */ pinMode(EN, OUTPUT); digitalWrite(EN, HIGH); + /* RS485 */ + //Serial1.begin(9600); + //pinMode(A3, OUTPUT); + //digitalWrite(A3, HIGH); + /* Initial Homing */ + Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true); } void loop() { - static double AZangle = 0; - + double *set_point; /* Read commands from serial */ - cmd_proc(AZangle); + set_point = cmd_proc(); /* Move Motor */ - dc_move(AZangle); + dc_move(set_point); } /* EasyComm 2 Protocol */ -void cmd_proc(double &angleAz) +double * cmd_proc() { + static double set_point[] = {0, 0}; /* Serial */ char buffer[BufferSize]; char incomingByte; @@ -82,54 +99,61 @@ void cmd_proc(double &angleAz) { if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L') { + double *pos; + pos = get_position(); /* Get position */ Serial.print("AZ"); - Serial.print(getposition(), 1); + Serial.print(pos[0], 1); Serial.print(" "); Serial.print("EL"); - Serial.println(0, 1); - Serial.println(az_angle_offset); + Serial.println(pos[1], 1); } else { /* Get the absolute value of angle */ rawData = strtok_r(Data, " " , &Data); - strncpy(data, rawData+2, 10); + strncpy(data, rawData + 2, 10); if (isNumber(data)) - angleAz = atof(data); + set_point[0] = atof(data); /* Get the absolute value of angle */ rawData = strtok_r(Data, " " , &Data); if (rawData[0] == 'E' && rawData[1] == 'L') { - strncpy(data, rawData+2, 10); + strncpy(data, rawData + 2, 10); if (isNumber(data)) - ; + set_point[1] = atof(data); } } } /* Stop Moving */ else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E') { - angleAz = getposition(); + double *pos; + pos = get_position(); /* Get position */ Serial.print("AZ"); - Serial.print(getposition(), 1); + Serial.print(pos[0], 1); Serial.print(" "); Serial.print("EL"); - Serial.println(0, 1); + Serial.println(pos[1], 1); + set_point[0] = pos[0]; + set_point[1] = pos[1]; } /* Reset the rotator */ else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T') { + double *pos; + pos = get_position(); /* Get position */ Serial.print("AZ"); - Serial.print(getposition(), 1); + Serial.print(pos[0], 1); Serial.print(" "); Serial.print("EL"); - Serial.println(0, 1); + Serial.println(pos[1], 1); /* Move to initial position */ - Homing(-MAX_AZ_ANGLE, false); - angleAz = 0; + Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false); + set_point[0] = 0; + set_point[1] = 0; } BufferCnt = 0; } @@ -139,27 +163,57 @@ void cmd_proc(double &angleAz) BufferCnt++; } } + return set_point; } /* Homing Function */ -void Homing(double AZangle, bool Init) +void Homing(double AZangle, double ELangle, bool Init) { int value_Home_AZ = DEFAULT_HOME_STATE; + int value_Home_EL = DEFAULT_HOME_STATE; boolean isHome_AZ = false; - double az_zero_angle = getposition(); + boolean isHome_EL = false; + double *zero_angle; + double *curr_angle; + double set_point[] = {0, 0}; - while(isHome_AZ == false ) + zero_angle = get_position(); + + if (zero_angle[0] > 0) + set_point[0] = AZangle; + else + set_point[0] = -AZangle; + + if (zero_angle[1] > 0) + set_point[1] = ELangle; + else + set_point[1] = -ELangle; + + while (isHome_AZ == false || isHome_EL == false) { - dc_move(AZangle); + dc_move(set_point); value_Home_AZ = digitalRead(HOME_AZ); + value_Home_EL = digitalRead(HOME_EL); + /* Change to LOW according to Home sensor */ if (value_Home_AZ == DEFAULT_HOME_STATE) { isHome_AZ = true; + curr_angle = get_position(); + set_point[0] = 0; if (Init) - az_angle_offset = getposition(); + az_angle_offset = curr_angle[0]; } - if (abs(getposition() - az_zero_angle) > MAX_AZ_ANGLE && !isHome_AZ) + if (value_Home_EL == DEFAULT_HOME_STATE) + { + isHome_EL = true; + curr_angle = get_position(); + set_point[1] = 0; + if (Init) + el_angle_offset = curr_angle[1]; + } + curr_angle = get_position(); + if ((abs(curr_angle[0] - zero_angle[0]) > MAX_AZ_ANGLE && !isHome_AZ) || (abs(curr_angle[1] - zero_angle[1]) > MAX_EL_ANGLE && !isHome_EL)) { /* set error */ break; @@ -167,75 +221,172 @@ void Homing(double AZangle, bool Init) } } -void dc_move(double set_point_az) +void dc_move(double set_point[]) { - static double u = 0; - static double angle_az = 0; - static double prev_angle_az = 0; - static double error = 0; - static double Iterm = 0; - static double Pterm = 0; - static double Dterm = 0; - double Kp = 2000; + static double u[] = {0, 0}; + static double *curr_pos; + static double prev_pos[] = {0, 0}; + static double error[] = {0, 0}; + static double Iterm[] = {0, 0}; + static double Pterm[] = {0, 0}; + static double Dterm[] = {0, 0}; + double Kp = 200; double Ki = 1; double Kd = 0; - double dt = 0.001; + double dt = 0.001; // calculate - angle_az = getposition(); - error = set_point_az - angle_az; + curr_pos = get_position(); + error[0] = set_point[0] - curr_pos[0]; + error[1] = set_point[1] - curr_pos[1]; - Pterm = Kp*error; + Pterm[0] = Kp * error[0]; + Pterm[1] = Kp * error[1]; - Iterm += Ki*error*dt; - if(Iterm > outMax) Iterm= outMax; - else if(Iterm < outMin) Iterm= outMin; + Iterm[0] += Ki * error[0] * dt; + Iterm[1] += Ki * error[1] * dt; + if (Iterm[0] > outMax) Iterm[0] = outMax; + else if (Iterm[0] < outMin) Iterm[0] = outMin; + if (Iterm[1] > outMax) Iterm[1] = outMax; + else if (Iterm[1] < outMin) Iterm[1] = outMin; - Dterm = Kd*(angle_az - prev_angle_az)/dt; - prev_angle_az = angle_az; + Dterm[0] = Kd * (curr_pos[0] - prev_pos[0]) / dt; + prev_pos[0] = curr_pos[0]; + Dterm[1] = Kd * (curr_pos[1] - prev_pos[1]) / dt; + prev_pos[1] = curr_pos[1]; - u = Pterm+Iterm+Dterm; + u[0] = Pterm[0] + Iterm[0] + Dterm[0]; + u[1] = Pterm[1] + Iterm[1] + Dterm[1]; - if (u >= 0) + if (u[0] >= 0) { - if (u > outMax) - u = outMax; - analogWrite(PWM1M1, u); + if (u[0] > outMax) + u[0] = outMax; + analogWrite(PWM1M1, u[0]); analogWrite(PWM2M1, 0); } else { - u = -u; - if ( u > outMax) - u = outMax; + u[0] = -u[0]; + if ( u[0] > outMax) + u[0] = outMax; analogWrite(PWM1M1, 0); - analogWrite(PWM2M1, u); + analogWrite(PWM2M1, u[0]); + } + + if (u[1] >= 0) + { + if (u[1] > outMax) + u[1] = outMax; + analogWrite(PWM1M2, u[1]); + analogWrite(PWM2M2, 0); + } + else + { + u[1] = -u[1]; + if ( u[1] > outMax) + u[1] = outMax; + analogWrite(PWM1M2, 0); + analogWrite(PWM2M2, u[1]); } } -/* get position from encoder */ -double getposition() +/* Read Encoders */ +double * get_position() { - double encoder_pos; - double delta_pos; - double real_pos; - static double prev_pos = 0; - int raw_pos; - static int n = 0; + int low_raw, high_raw, status_val; + double raw_pos = 0; + double delta_raw_pos = 0; + static double raw_prev_pos[] = {0, 0}; + static double real_pos[] = {0, 0}; + static int n[] = {0, 0}; - raw_pos = readPosition(); - if (raw_pos >= 0) + /* Axis 1*/ + /* Read Raw Angle Low Byte */ + Wire.beginTransmission(as5601_adr); + Wire.write(raw_ang_low); + Wire.endTransmission(); + Wire.requestFrom(as5601_adr, 1); + while(Wire.available() == 0); + low_raw = Wire.read(); + /* Read Raw Angle High Byte */ + Wire.beginTransmission(as5601_adr); + Wire.write(raw_ang_high); + Wire.endTransmission(); + Wire.requestFrom(as5601_adr, 1); + while(Wire.available() == 0); + high_raw = Wire.read(); + high_raw = high_raw << 8; + high_raw = high_raw | low_raw; + /* Read Status Bits */ + Wire.beginTransmission(as5601_adr); + Wire.write(status_reg); + Wire.endTransmission(); + Wire.requestFrom(as5601_adr, 1); + while(Wire.available() == 0); + status_val = Wire.read(); + /* Check the status register */ + if ((status_val & 0x20) && !(status_val & 0x10) && !(status_val & 0x08)) { - encoder_pos = ((float)raw_pos / 1024.0) * 360.0; - delta_pos = prev_pos - encoder_pos; - if (delta_pos > 180) - n++; - else if (delta_pos < -180) - n--; - real_pos = ((encoder_pos + 360*n)/RATIO) - az_angle_offset; - prev_pos = encoder_pos; + if (high_raw >= 0) + { + raw_pos = (double)high_raw*0.0879; + delta_raw_pos = raw_prev_pos[0] - raw_pos; + if (delta_raw_pos > 180) + n[0]++; + else if (delta_raw_pos < -180) + n[0]--; + real_pos[0] = ((raw_pos + 360 * n[0]) / RATIO) - az_angle_offset; + raw_prev_pos[0] = raw_pos; + } + else + ; /* set error */ } else - ; /* set error */ + ; /* set error */ + + /* Axis 2 */ + /* Read Raw Angle Low Byte */ + i2c_soft.beginTransmission(as5601_adr); + i2c_soft.write(raw_ang_low); + i2c_soft.endTransmission(); + i2c_soft.requestFrom(as5601_adr); + low_raw = i2c_soft.readLast(); + i2c_soft.endTransmission(); + /* Read Raw Angle High Byte */ + i2c_soft.beginTransmission(as5601_adr); + i2c_soft.write(raw_ang_high); + i2c_soft.endTransmission(); + i2c_soft.requestFrom(as5601_adr); + high_raw = i2c_soft.readLast(); + i2c_soft.endTransmission(); + high_raw = high_raw << 8; + high_raw = high_raw | low_raw; + /* Read Status Bits */ + i2c_soft.beginTransmission(as5601_adr); + i2c_soft.write(status_reg); + i2c_soft.endTransmission(); + i2c_soft.requestFrom(as5601_adr); + status_val = i2c_soft.readLast(); + i2c_soft.endTransmission(); + /* Check the status register */ + if ((status_val & 0x20) && !(status_val & 0x10) && !(status_val & 0x08)) + { + if (high_raw >= 0) + { + raw_pos = (double)high_raw*0.0879; + delta_raw_pos = raw_prev_pos[1] - raw_pos; + if (delta_raw_pos > 180) + n[1]++; + else if (delta_raw_pos < -180) + n[1]--; + real_pos[1] = ((raw_pos + 360 * n[1]) / RATIO) - el_angle_offset; + raw_prev_pos[1] = raw_pos; + } + else + ; /* set error */ + } + else + ; /* set error */ return real_pos; } @@ -248,67 +399,6 @@ boolean isNumber(char *input) if (isalpha(input[i])) return false; } - return true; -} - -/* Code from http://reprap.org/wiki/Magnetic_Rotary_Encoder_1.0 */ - -/* read the current angular position */ -int readPosition() -{ - unsigned int position = 0; - - /* shift in our data */ - digitalWrite(SELECT_PIN, LOW); - delayMicroseconds(0.5); - byte d1 = shiftIn(DATA_PIN, CLOCK_PIN); - byte d2 = shiftIn(DATA_PIN, CLOCK_PIN); - digitalWrite(SELECT_PIN, HIGH); - - /* get our position variable */ - position = d1; - position = position << 8; - position |= d2; - - position = position >> 6; - - /* check the offset compensation flag: 1 == started up */ - if (!(d2 & B00100000)) - position = -1; - - /* check the cordic overflow flag: 1 = error */ - if (d2 & B00010000) - position = -2; - - /* check the linearity alarm: 1 = error */ - if (d2 & B00001000) - position = -3; - - /* check the magnet range: 11 = error */ - if ((d2 & B00000110) == B00000110) - position = -4; - - return position; -} - -/* read in a byte of data from the digital input of the board. */ -byte shiftIn(byte data_pin, byte clock_pin) -{ - byte data = 0; - - for (int i=7; i>=0; i--) - { - digitalWrite(clock_pin, LOW); - delayMicroseconds(0.5); - digitalWrite(clock_pin, HIGH); - delayMicroseconds(0.5); - digitalWrite(clock_pin, LOW); - - byte bit = digitalRead(data_pin); - data |= (bit << i); - - } - - return data; + return true; } diff --git a/firmware/libraries/SoftI2CMaster/LICENSE.txt b/firmware/libraries/SoftI2CMaster/LICENSE.txt new file mode 100644 index 0000000..91a4dc6 --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/LICENSE.txt @@ -0,0 +1,33 @@ + +Copyright (c) Tod E. Kurt, 2010-2015 + +--- + +LICENSE: +Creative Commons - Attribution - ShareAlike 3.0 +http://creativecommons.org/licenses/by-sa/3.0/ +See why we chose this license: http://www.inmojo.com/licenses/ + +This work is licensed under the Creative Commons Attribution-ShareAlike 3.0 Unported License. To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/ or send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA. + + +This is a human-readable summary of the full license. + +You are free: +- to Share - to copy, distribute and transmit the work, and +- to Remix - to adapt the work + +Under the following conditions: +- Attribution - You must attribute the work in the manner specified by the author or licensor (but not in any way that suggests that they endorse you or your use of the work.) +- Share Alike - If you alter, transform, or build upon this work, you may distribute the resulting work only under the same, similar or a compatible license. + +With the understanding that: +- Waiver - Any of the above conditions can be waived if you get permission from the copyright holder. +- Other Rights - In no way are any of the following rights affected by the license: +-- your fair dealing or fair use rights; +-- the author's moral rights; and +-- rights other persons may have either in the work itself or in how the work is used, such as publicity or privacy rights. + +Notice - For any reuse or distribution, you must make clear to others the license terms of this work. The best way to do that is with a link to +http://creativecommons.org/licenses/by-sa/3.0/ + diff --git a/firmware/libraries/SoftI2CMaster/README.TXT b/firmware/libraries/SoftI2CMaster/README.TXT new file mode 100644 index 0000000..7b0e841 --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/README.TXT @@ -0,0 +1,16 @@ + +SoftI2CMaster + +2010-2012, Tod E. Kurt, http://todbot.com/blog/ +2014, by Testato: update library and examples for follow Wire’s API of Arduino IDE 1.x + + +SoftI2CMaster is an Arduino library that implements +a simple "bit-bang" software-only I2C (aka "TWI") Master. + +This lets you use any two Arduino pins to be the SDA & SCL +pins needed to communicate with I2C devices like BlinkMs. + +SoftI2CMaster was originally in the BlinkMSoftI2CDemo example +in http://code.google.com/p/blinkm-projects/ + diff --git a/firmware/libraries/SoftI2CMaster/SoftI2CMaster.cpp b/firmware/libraries/SoftI2CMaster/SoftI2CMaster.cpp new file mode 100644 index 0000000..f836696 --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/SoftI2CMaster.cpp @@ -0,0 +1,328 @@ +/* + * SoftI2CMaster.cpp -- Multi-instance software I2C Master library + * + * + * 2010-12 Tod E. Kurt, http://todbot.com/blog/ + * + * This code takes some tricks from: + * http://codinglab.blogspot.com/2008/10/i2c-on-avr-using-bit-banging.html + * + * 2014, by Testato: update library and examples for follow Wire’s API of Arduino IDE 1.x + * + */ + +#if (ARDUINO >= 100) +#include +#else +#include +#endif + +#include "SoftI2CMaster.h" + +#include +#include + +#define i2cbitdelay 50 + +#define I2C_ACK 1 +#define I2C_NAK 0 + + +#define i2c_scl_release() \ + *_sclDirReg &=~ _sclBitMask +#define i2c_sda_release() \ + *_sdaDirReg &=~ _sdaBitMask + +// sets SCL low and drives output +#define i2c_scl_lo() \ + *_sclPortReg &=~ _sclBitMask; \ + *_sclDirReg |= _sclBitMask; + +// sets SDA low and drives output +#define i2c_sda_lo() \ + *_sdaPortReg &=~ _sdaBitMask; \ + *_sdaDirReg |= _sdaBitMask; + +// set SCL high and to input (releases pin) (i.e. change to input,turnon pullup) +#define i2c_scl_hi() \ + *_sclDirReg &=~ _sclBitMask; \ + if(usePullups) { *_sclPortReg |= _sclBitMask; } + +// set SDA high and to input (releases pin) (i.e. change to input,turnon pullup) +#define i2c_sda_hi() \ + *_sdaDirReg &=~ _sdaBitMask; \ + if(usePullups) { *_sdaPortReg |= _sdaBitMask; } + + +// +// Constructor +// +SoftI2CMaster::SoftI2CMaster() +{ + // do nothing, use setPins() later +} +// +SoftI2CMaster::SoftI2CMaster(uint8_t sclPin, uint8_t sdaPin) +{ + setPins(sclPin, sdaPin, true); + i2c_init(); +} + +// +SoftI2CMaster::SoftI2CMaster(uint8_t sclPin, uint8_t sdaPin, uint8_t pullups) +{ + setPins(sclPin, sdaPin, pullups); + i2c_init(); +} + +// +// Turn Arduino pin numbers into PORTx, DDRx, and PINx +// +void SoftI2CMaster::setPins(uint8_t sclPin, uint8_t sdaPin, uint8_t pullups) +{ + uint8_t port; + + usePullups = pullups; + + _sclPin = sclPin; + _sdaPin = sdaPin; + + _sclBitMask = digitalPinToBitMask(sclPin); + _sdaBitMask = digitalPinToBitMask(sdaPin); + + port = digitalPinToPort(sclPin); + _sclPortReg = portOutputRegister(port); + _sclDirReg = portModeRegister(port); + + port = digitalPinToPort(sdaPin); + _sdaPortReg = portOutputRegister(port); + _sdaDirReg = portModeRegister(port); + +} + +// +// +// +uint8_t SoftI2CMaster::beginTransmission(uint8_t address) +{ + i2c_start(); + uint8_t rc = i2c_write((address<<1) | 0); // clr read bit + return rc; +} + +// +uint8_t SoftI2CMaster::requestFrom(uint8_t address) +{ + i2c_start(); + uint8_t rc = i2c_write((address<<1) | 1); // set read bit + return rc; +} +// +uint8_t SoftI2CMaster::requestFrom(int address) +{ + return requestFrom( (uint8_t) address); +} + +// +uint8_t SoftI2CMaster::beginTransmission(int address) +{ + return beginTransmission((uint8_t)address); +} + +// +// +// +uint8_t SoftI2CMaster::endTransmission(void) +{ + i2c_stop(); + //return ret; // FIXME + return 0; +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +uint8_t SoftI2CMaster::write(uint8_t data) +{ + return i2c_write(data); +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +void SoftI2CMaster::write(uint8_t* data, uint8_t quantity) +{ + for(uint8_t i = 0; i < quantity; ++i){ + write(data[i]); + } +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +void SoftI2CMaster::write(char* data) +{ + write((uint8_t*)data, strlen(data)); +} + +// must be called in: +// slave tx event callback +// or after beginTransmission(address) +void SoftI2CMaster::write(int data) +{ + write((uint8_t)data); +} + +//-------------------------------------------------------------------- + + +void SoftI2CMaster::i2c_writebit( uint8_t c ) +{ + if ( c > 0 ) { + i2c_sda_hi(); + } else { + i2c_sda_lo(); + } + + i2c_scl_hi(); + _delay_us(i2cbitdelay); + + i2c_scl_lo(); + _delay_us(i2cbitdelay); + + if ( c > 0 ) { + i2c_sda_lo(); + } + _delay_us(i2cbitdelay); +} + +// +uint8_t SoftI2CMaster::i2c_readbit(void) +{ + i2c_sda_hi(); + i2c_scl_hi(); + _delay_us(i2cbitdelay); + + uint8_t port = digitalPinToPort(_sdaPin); + volatile uint8_t* pinReg = portInputRegister(port); + uint8_t c = *pinReg; // I2C_PIN; + + i2c_scl_lo(); + _delay_us(i2cbitdelay); + + return ( c & _sdaBitMask) ? 1 : 0; +} + +// Inits bitbanging port, must be called before using the functions below +// +void SoftI2CMaster::i2c_init(void) +{ + //I2C_PORT &=~ (_BV( I2C_SDA ) | _BV( I2C_SCL )); + //*_sclPortReg &=~ (_sdaBitMask | _sclBitMask); + i2c_sda_hi(); + i2c_scl_hi(); + + _delay_us(i2cbitdelay); +} + +// Send a START Condition +// +void SoftI2CMaster::i2c_start(void) +{ + // set both to high at the same time + //I2C_DDR &=~ (_BV( I2C_SDA ) | _BV( I2C_SCL )); + //*_sclDirReg &=~ (_sdaBitMask | _sclBitMask); + i2c_sda_hi(); + i2c_scl_hi(); + + _delay_us(i2cbitdelay); + + i2c_sda_lo(); + _delay_us(i2cbitdelay); + + i2c_scl_lo(); + _delay_us(i2cbitdelay); +} + +void SoftI2CMaster::i2c_repstart(void) +{ + // set both to high at the same time (releases drive on both lines) + //I2C_DDR &=~ (_BV( I2C_SDA ) | _BV( I2C_SCL )); + //*_sclDirReg &=~ (_sdaBitMask | _sclBitMask); + i2c_sda_hi(); + i2c_scl_hi(); + + i2c_scl_lo(); // force SCL low + _delay_us(i2cbitdelay); + + i2c_sda_release(); // release SDA + _delay_us(i2cbitdelay); + + i2c_scl_release(); // release SCL + _delay_us(i2cbitdelay); + + i2c_sda_lo(); // force SDA low + _delay_us(i2cbitdelay); +} + +// Send a STOP Condition +// +void SoftI2CMaster::i2c_stop(void) +{ + i2c_scl_hi(); + _delay_us(i2cbitdelay); + + i2c_sda_hi(); + _delay_us(i2cbitdelay); +} + +// write a byte to the I2C slave device +// +uint8_t SoftI2CMaster::i2c_write( uint8_t c ) +{ + for ( uint8_t i=0;i<8;i++) { + i2c_writebit( c & 128 ); + c<<=1; + } + + return i2c_readbit(); +} + +// read a byte from the I2C slave device +// +uint8_t SoftI2CMaster::i2c_read( uint8_t ack ) +{ + uint8_t res = 0; + + for ( uint8_t i=0;i<8;i++) { + res <<= 1; + res |= i2c_readbit(); + } + + if ( ack ) + i2c_writebit( 0 ); + else + i2c_writebit( 1 ); + + _delay_us(i2cbitdelay); + + return res; +} + +// FIXME: this isn't right, surely +uint8_t SoftI2CMaster::read( uint8_t ack ) +{ + return i2c_read( ack ); +} + +// +uint8_t SoftI2CMaster::read() +{ + return i2c_read( I2C_ACK ); +} + +// +uint8_t SoftI2CMaster::readLast() +{ + return i2c_read( I2C_NAK ); +} diff --git a/firmware/libraries/SoftI2CMaster/SoftI2CMaster.h b/firmware/libraries/SoftI2CMaster/SoftI2CMaster.h new file mode 100644 index 0000000..260ce90 --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/SoftI2CMaster.h @@ -0,0 +1,68 @@ +/* + * SoftI2CMaster.h -- Multi-instance software I2C Master library + * + * 2010-2012 Tod E. Kurt, http://todbot.com/blog/ + * 2014, by Testato: update library and examples for follow Wire’s API of Arduino IDE 1.x + * + */ + +#ifndef SoftI2CMaster_h +#define SoftI2CMaster_h + +#include + +#define _SOFTI2CMASTER_VERSION 13 // software version of this library + + +class SoftI2CMaster +{ + +private: + // per object data + uint8_t _sclPin; + uint8_t _sdaPin; + uint8_t _sclBitMask; + uint8_t _sdaBitMask; + volatile uint8_t *_sclPortReg; + volatile uint8_t *_sdaPortReg; + volatile uint8_t *_sclDirReg; + volatile uint8_t *_sdaDirReg; + + uint8_t usePullups; + + // private methods + + void i2c_writebit( uint8_t c ); + uint8_t i2c_readbit(void); + void i2c_init(void); + void i2c_start(void); + void i2c_repstart(void); + void i2c_stop(void); + uint8_t i2c_write( uint8_t c ); + uint8_t i2c_read( uint8_t ack ); + +public: + // public methods + SoftI2CMaster(); + SoftI2CMaster(uint8_t sclPin, uint8_t sdaPin); + SoftI2CMaster(uint8_t sclPin, uint8_t sdaPin, uint8_t usePullups); + + void setPins(uint8_t sclPin, uint8_t sdaPin, uint8_t usePullups); + + uint8_t beginTransmission(uint8_t address); + uint8_t beginTransmission(int address); + uint8_t endTransmission(void); + uint8_t write(uint8_t); + void write(uint8_t*, uint8_t); + void write(int); + void write(char*); + void begin(void) {return;}; + uint8_t requestFrom(int address); + uint8_t requestFrom(uint8_t address); + uint8_t read( uint8_t ack ); + uint8_t read(); + uint8_t readLast(); + +}; + +#endif diff --git a/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkMSoftI2CDemo.ino b/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkMSoftI2CDemo.ino new file mode 100644 index 0000000..b44150f --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkMSoftI2CDemo.ino @@ -0,0 +1,95 @@ +/* + * BlinkMSoftI2CDemo -- very simply demonstrate Softi2CMaster library + * + * + * + * 2010 Tod E. Kurt, http://todbot.com/blog/ + * 2014, by Testato: update library and examples for follow Wire’s API of Arduino IDE 1.x + * + */ + +const boolean testingI2CReads = true; + +// Choose any pins you want. The pins below let you plug in a BlinkM directly +const byte sclPin = 7; // digital pin 7 wired to 'c' on BlinkM +const byte sdaPin = 6; // digital pin 6 wired to 'd' on BlinkM +const byte pwrPin = 5; // digital pin 5 wired to '+' on BlinkM +const byte gndPin = 4; // digital pin 4 wired to '-' on BlinkM + +#include "SoftI2CMaster.h" +#include "BlinkM_funcs_soft.h" + + +byte blinkm_addr = 9; + +// +void setup() +{ + Serial.begin( 19200 ); + Serial.println(F("BlinkMSoftI2CDemo")); + + BlinkM_begin( sclPin, sdaPin, pwrPin, gndPin ); + + delay(100); + + BlinkM_off(0); + BlinkM_setFadeSpeed( blinkm_addr, 5); + + for( int i=0; i< 10; i++ ) { // flash the blinkms + BlinkM_setRGB( blinkm_addr, 255,255,255 ); + delay(20); + BlinkM_setRGB( blinkm_addr, 0,0,0 ); + delay(20); + } + + if( testingI2CReads ) { + Serial.print(F("BlinkM version: ")); + int num = BlinkM_getVersion( blinkm_addr ); + char major_version = (char)(num>>8); + char minor_version = (char)(num&0xff); + Serial.print( major_version ); + Serial.println( minor_version ); + if( major_version == -1 ) { + Serial.println(F("\nERROR: couldn't find a BlinkM\n")); + } + } +} + +void loop() +{ + byte r = random(255); + byte g = random(255); + byte b = random(255); + + Serial.print(F("Setting r,g,b:")); Serial.print(r,HEX); + Serial.print(","); Serial.print(g,HEX); + Serial.print(","); Serial.println(b,HEX); + + BlinkM_setRGB( blinkm_addr, r,g,b ); + delay(50); + BlinkM_fadeToRGB( blinkm_addr, 0,0,0 ); + + if( testingI2CReads ) { + for( int i=0; i<10; i++ ) { + showCurrentColor(); + delay(100); + } + } + else { + delay(1000); + } +} + +// +void showCurrentColor() +{ + byte r,g,b; + BlinkM_getRGBColor( blinkm_addr, &r,&g,&b); + + Serial.print(F(" r,g,b:")); Serial.print(r,HEX); + Serial.print(","); Serial.print(g,HEX); + Serial.print(","); Serial.println(b,HEX); +} + + + diff --git a/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkM_funcs_soft.h b/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkM_funcs_soft.h new file mode 100644 index 0000000..b39e74d --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/examples/BlinkMSoftI2CDemo/BlinkM_funcs_soft.h @@ -0,0 +1,127 @@ +/** + * BlinkM_funcs_soft.h -- an extremely cut-down version of BlinkM_funcs.h + * for use with SoftI2CMaster library + * + * + * 2010 Tod E. Kurt, http://todbot.com/blog/ + * 2014, by Testato: update library and examples for follow Wire’s API of Arduino IDE 1.x + * + */ + +int errcnt; + +#include "SoftI2CMaster.h" + +SoftI2CMaster i2c = SoftI2CMaster(); + + +// set which arbitrary I/O pins will be "power" and "ground" for the BlinkM +static void BlinkM_setPowerPins(byte pwrpin, byte gndpin) +{ + pinMode(pwrpin, OUTPUT); + pinMode(gndpin, OUTPUT); + digitalWrite(pwrpin, HIGH); + digitalWrite(gndpin, LOW); + delay(10); // wait for power to stabilize +} + +// set which arbitrary I/O pins will be BlinkMs SCL and SDA +// note, this sets the internal pull-up resistors +static void BlinkM_begin( byte sclpin, byte sdapin ) +{ + i2c.setPins( sclPin,sdaPin, true ); +} + +// start up a BlinkM with four arbitrary I/O pins +static void BlinkM_begin( byte sclpin, byte sdapin, byte pwrpin, byte gndpin) +{ + BlinkM_setPowerPins( pwrpin, gndpin ); + i2c.setPins( sclPin,sdaPin, true ); +} + +// ----------------------------------------------------------------------------- +// many BlinkM commands are 3 arguments in length, here's a generalized form +static void BlinkM_sendCmd3( uint8_t addr, uint8_t c, uint8_t a1, uint8_t a2, uint8_t a3 ) +{ + if( i2c.beginTransmission( addr ) == 0 ) { + ++errcnt; + //Serial.println( errcnt); // FIXME + } + i2c.write( c ); + i2c.write( a1 ); + i2c.write( a2 ); + i2c.write( a3 ); + i2c.endTransmission(); +} + +// other BlinkM commands have a single argument +static void BlinkM_sendCmd1( uint8_t addr, uint8_t c, uint8_t a1) +{ + if( i2c.beginTransmission( addr ) == 0 ) { + ++errcnt; + //Serial.println( errcnt); // FIXME + } + i2c.write( c ); + i2c.write( a1 ); +} + +static void BlinkM_stopScript(uint8_t addr) +{ + i2c.beginTransmission( addr ); + i2c.write( 'o' ); + i2c.endTransmission(); +} + +static void BlinkM_setFadeSpeed( uint8_t addr, uint8_t f) +{ + BlinkM_sendCmd1( addr, 'f', f ); +} + +static void BlinkM_fadeToRGB( uint8_t addr, uint8_t r, uint8_t g, uint8_t b ) +{ + BlinkM_sendCmd3( addr, 'c', r,g,b ); +} + +static void BlinkM_setRGB( uint8_t addr, uint8_t r, uint8_t g, uint8_t b ) +{ + BlinkM_sendCmd3( addr, 'n', r,g,b ); +} + +static void BlinkM_off(uint8_t addr) +{ + BlinkM_stopScript( addr ); + BlinkM_setFadeSpeed(addr,10); + BlinkM_setRGB(addr, 0,0,0 ); +} + +// Get the BlinkM firmware version +static int BlinkM_getVersion(byte addr) +{ + i2c.beginTransmission( addr ); + i2c.write( 'Z' ); + i2c.endTransmission(); + + i2c.requestFrom( addr ); + uint8_t major_ver = i2c.read(); + uint8_t minor_ver = i2c.readLast(); + i2c.endTransmission(); + return (major_ver<<8) + minor_ver; +} + +// +static void BlinkM_getRGBColor(byte addr, byte* r, byte* g, byte* b) +{ + i2c.beginTransmission(addr); + i2c.write('g'); + i2c.endTransmission(); + + i2c.requestFrom( addr ); + *r = i2c.read(); + *g = i2c.read(); + *b = i2c.readLast(); + i2c.endTransmission(); +} + + + + diff --git a/firmware/libraries/SoftI2CMaster/examples/MMA8452Q_Example_SoftI2C/MMA8452Q_Example_SoftI2C.ino b/firmware/libraries/SoftI2CMaster/examples/MMA8452Q_Example_SoftI2C/MMA8452Q_Example_SoftI2C.ino new file mode 100644 index 0000000..f728585 --- /dev/null +++ b/firmware/libraries/SoftI2CMaster/examples/MMA8452Q_Example_SoftI2C/MMA8452Q_Example_SoftI2C.ino @@ -0,0 +1,317 @@ +/* MMA8452Q Example Code + by: Jim Lindblom + SparkFun Electronics + date: October 13, 2011 + license: Creative Commons Share-Alike v3.0 (CC BY-SA 3.0) + + This code should provide example usage for most features of + the MMA8452Q 3-axis, I2C accelerometer. In the loop function + the accelerometer interrupt outputs will be polled, and either + the x/y/z accel data will be output, or single/double-taps, + portrait/landscape changes will be announced. + + The skeleton is here, feel free to cut/paste what code you need. + Play around with the settings in initMMA8452Q. Try running the + code without printing the accel values, to really appreciate + the single/double-tap and portrait landscape functions. The + P/L stuff is really neat, something not many accelerometers have. + + Hardware setup: + MMA8452 Breakout ------------ Arduino + 3.3V --------------------- 3.3V + SDA ----------------------- A4 + SCL ----------------------- A5 + INT2 ---------------------- D3 + INT1 ---------------------- D2 + GND ---------------------- GND + + SDA and SCL should have external pull-up resistors (to 3.3V). + 10k resistors worked for me. They should be on the breakout + board. + + Note: The MMA8452 is an I2C sensor, however this code does + not make use of the Arduino Wire library. Because the sensor + is not 5V tolerant, we can't use the internal pull-ups used + by the Wire library. Instead use the included i2c.h file. +*/ + +//#include "i2c.h" // not the wire library, can't use pull-ups +#include "SoftI2CMaster.h" + +//const int sdaPin = A4; +//const int sclPin = A5; +const int sdaPin = 4; +const int sclPin = 5; +SoftI2CMaster i2c = SoftI2CMaster( sclPin, sdaPin, 0 ); + +#define SA0 1 // Breakout board defaults to 1, set to 0 if SA0 jumper is set +#if SA0 + #define MMA8452_ADDRESS 0x1D // SA0 is high, 0x1C if low +#else + #define MMA8452_ADDRESS 0x1C +#endif + +/* Set the scale below either 2, 4 or 8*/ +const byte scale = 2; // Sets full-scale range to +/-2, 4, or 8g. Used to calc real g values. +/* Set the output data rate below. Value should be between 0 and 7*/ +const byte dataRate = 0; // 0=800Hz, 1=400, 2=200, 3=100, 4=50, 5=12.5, 6=6.25, 7=1.56 + +/* Pin definitions */ +int int1Pin = 2; // These can be changed, 2 and 3 are the Arduinos ext int pins +int int2Pin = 3; + +byte data[6]; // x/y/z accel register data store here +int accelCount[3]; // Stores the 12-bit signed value +float accel[3]; // Stores the real accel value in g's + +void setup() +{ + byte c; + + Serial.begin(115200); + + /* Set up the interrupt pins, they're set as active high, push-pull */ + pinMode(int1Pin, INPUT); + digitalWrite(int1Pin, LOW); + pinMode(int2Pin, INPUT); + digitalWrite(int2Pin, LOW); + + /* Read the WHO_AM_I register, this is a good test of communication */ + c = readRegister(0x0D); // Read WHO_AM_I register + if (c == 0x2A) // WHO_AM_I should always be 0x2A + { + initMMA8452(scale, dataRate); // init the accelerometer if communication is good + Serial.println(F("MMA8452Q is online...")); + } + else + { + Serial.print(F("Could not connect to MMA8452Q: 0x")); + Serial.println(c, HEX); + while (1) // Loop forever if communication doesn't happen + ; + } +} + +void loop() +{ + static byte source; + + /* If int1 goes high, all data registers have new data */ + + if (digitalRead(int1Pin)) // Interrupt pin, should probably attach to interrupt function + //if (readRegister(0x00)&0x7) // Polling, you can use this instead of the interrupt pins + { + readRegisters(0x01, 6, &data[0]); // Read the six data registers into data array + + // For loop to calculate 12-bit ADC and g value for each axis + for (int i=0; i<6; i+=2) + { + accelCount[i/2] = ((data[i] << 8) | data[i+1]) >> 4; // Turn the MSB and LSB into a 12-bit value + if (data[i] > 0x7F) + { // If the number is negative, we have to make it so manually (no 12-bit data type) + accelCount[i/2] = ~accelCount[i/2] + 1; + accelCount[i/2] *= -1; // Transform into negative 2's complement # + } + accel[i/2] = (float) accelCount[i/2]/((1<<12)/(2*scale)); // get actual g value, this depends on scale being set + } + + // For loop to print out values + + for (int i=0; i<3; i++) + { + Serial.print(accel[i], 4); // Print g values + //Serial.print(accelCount[i], DEC); // Print adc count values, feel free to uncomment this line + Serial.print("\t\t"); + } + Serial.println(); + + } + + + /* If int2 goes high, either p/l has changed or there's been a single/double tap */ + if (digitalRead(int2Pin)) + { + source = readRegister(0x0C); // Read the interrupt source reg. + if ((source & 0x10)==0x10) // If the p/l bit is set, go check those registers + portraitLandscapeHandler(); + else if ((source & 0x08)==0x08) // Otherwise, if tap register is set go check that + tapHandler(); + delay(100); // Delay here for a little printing visibility, make it longer, or delete it + } + delay(20); +} + +/* This function will read the status of the tap source register. + And print if there's been a single or double tap, and on what + axis. */ +void tapHandler() +{ + byte source = readRegister(0x22); // Reads the PULSE_SRC register + + if ((source & 0x10)==0x10) // If AxX bit is set + { + if ((source & 0x08)==0x08) // If DPE (double puls) bit is set + Serial.print(" 2 X"); + else + Serial.print("1 X"); + + if ((source & 0x01)==0x01) // If PoIX is set + Serial.println(" +"); + else + Serial.println(" -"); + } + if ((source & 0x20)==0x20) // If AxY bit is set + { + if ((source & 0x08)==0x08) // If DPE (double puls) bit is set + Serial.print(" 2 Y"); + else + Serial.print("1 Y"); + + if ((source & 0x02)==0x02) // If PoIY is set + Serial.println(" +"); + else + Serial.println(" -"); + } + if ((source & 0x40)==0x40) // If AxZ bit is set + { + if ((source & 0x08)==0x08) // If DPE (double puls) bit is set + Serial.print(" 2 Z"); + else + Serial.print("1 Z"); + if ((source & 0x04)==0x04) // If PoIZ is set + Serial.println(" +"); + else + Serial.println(" -"); + } +} + +/* This function will read the p/l source register and + print what direction the sensor is now facing */ +void portraitLandscapeHandler() +{ + byte pl = readRegister(0x10); // Reads the PL_STATUS register + switch((pl&0x06)>>1) // Check on the LAPO[1:0] bits + { + case 0: + Serial.print(F("Portrait up, ")); + break; + case 1: + Serial.print(F("Portrait Down, ")); + break; + case 2: + Serial.print(F("Landscape Right, ")); + break; + case 3: + Serial.print(F("Landscape Left, ")); + break; + } + if (pl&0x01) // Check the BAFRO bit + Serial.print(F("Back")); + else + Serial.print(F("Front")); + if (pl&0x40) // Check the LO bit + Serial.print(F(", Z-tilt!")); + Serial.println(); +} + +/* Initialize the MMA8452 registers + See the many application notes for more info on setting + all of these registers: + http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MMA8452Q + + Feel free to modify any values, these are settings that work well for me. +*/ +void initMMA8452(byte fsr, byte dataRate) +{ + MMA8452Standby(); // Must be in standby to change registers + + /* Set up the full scale range to 2, 4, or 8g. */ + if ((fsr==2)||(fsr==4)||(fsr==8)) + writeRegister(0x0E, fsr >> 2); + else + writeRegister(0x0E, 0); + /* Setup the 3 data rate bits, from 0 to 7 */ + writeRegister(0x2A, readRegister(0x2A) & ~(0x38)); + if (dataRate <= 7) + writeRegister(0x2A, readRegister(0x2A) | (dataRate << 3)); + /* Set up portrait/landscap registers */ + writeRegister(0x11, 0x40); // Enable P/L + writeRegister(0x13, 0x14); // 29deg z-lock, + writeRegister(0x14, 0x84); // 45deg thresh, 14deg hyst + writeRegister(0x12, 0x05); // debounce counter at 100ms + /* Set up single and double tap */ + writeRegister(0x21, 0x7F); // enable single/double taps on all axes + writeRegister(0x23, 0x20); // x thresh at 2g + writeRegister(0x24, 0x20); // y thresh at 2g + writeRegister(0x25, 0x8); // z thresh at .5g + writeRegister(0x26, 0x30); // 60ms time limit, the min/max here is very dependent on output data rate + writeRegister(0x27, 0x28); // 200ms between taps min + writeRegister(0x28, 0xFF); // 1.275s (max value) between taps max + /* Set up interrupt 1 and 2 */ + writeRegister(0x2C, 0x02); // Active high, push-pull + writeRegister(0x2D, 0x19); // DRDY int enabled, P/L enabled + writeRegister(0x2E, 0x01); // DRDY on INT1, P/L on INT2 + + MMA8452Active(); // Set to active to start reading +} + +/* Sets the MMA8452 to standby mode. + It must be in standby to change most register settings */ +void MMA8452Standby() +{ + byte c = readRegister(0x2A); + writeRegister(0x2A, c & ~(0x01)); +} + +/* Sets the MMA8452 to active mode. + Needs to be in this mode to output data */ +void MMA8452Active() +{ + byte c = readRegister(0x2A); + writeRegister(0x2A, c | 0x01); +} + +/* Read i registers sequentially, starting at address + into the dest byte arra */ +void readRegisters(byte address, int i, byte * dest) +{ + i2c.beginTransmission( MMA8452_ADDRESS ); + i2c.write( address ); + i2c.endTransmission(); + + i2c.requestFrom( MMA8452_ADDRESS ); + int j; + for( j=0; j