void loop()
{
updatelcd();
_XJoyLeft = digitalReadFast(X_JOY_LEFT);
_XJoyRight = digitalReadFast(X_JOY_RIGHT);
_YJoyUp = digitalReadFast(Y_JOY_UP);
_YJoyDown = digitalReadFast(Y_JOY_DOWN);
analogWrite(X_PWM_PIN, 0);
analogWrite(Y_PWM_PIN, 0);
if(!_XJoyLeft)
{
digitalWrite(X_DIR_PIN, HIGH); //Establishes forward direction of Channel A
analogWrite(X_PWM_PIN, 255); //Spins the motor on Channel A at full speed
}
if(!_XJoyRight)
{
digitalWrite(X_DIR_PIN,LOW); //Establishes backward direction of Channel A
analogWrite(X_PWM_PIN, 255); //Spins the motor on Channel A at full speed
}
if(!_YJoyUp)
{
digitalWrite(Y_DIR_PIN,HIGH); //Establishes backward direction of Channel A
analogWrite(Y_PWM_PIN, 255); //Spins the motor on Channel A at full speed
}
if(!_YJoyDown)
{
digitalWrite(Y_DIR_PIN,LOW); //Establishes backward direction of Channel A
analogWrite(Y_PWM_PIN, 255); //Spins the motor on Channel A at full speed
}
delay(200);
}
void Encoder::initialize()
{
pinModeFast(Utility::PIN_EncoderChannelA, INPUT);
pinModeFast(Utility::PIN_EncoderChannelB, INPUT);
encoderValues = (digitalReadFast(Utility::PIN_EncoderChannelA) << 1) | digitalReadFast(Utility::PIN_EncoderChannelB);
// Sets ISR for external interrupt on pin 2
attachInterrupt(0, quadratureDecoderISR, RISING);
}
boolean PJON::can_start() {
pinModeFast(_input_pin, INPUT);
for(uint8_t i = 0; i < 9; i++) {
if(digitalReadFast(_input_pin))
return false;
delayMicroseconds(BIT_WIDTH);
}
if(digitalReadFast(_input_pin))
return false;
return true;
}
byte SPIPump(byte data) {
byte receivedData=0;
for (int8_t i=7; i>=0; i--) {
receivedData <<= 1;
if (data & (1<<i)) {
digitalWriteFast(WLAN_MOSI, HIGH);
}
else {
digitalWriteFast(WLAN_MOSI, LOW);
}
digitalWriteFast(WLAN_SCK, HIGH);
asm volatile("nop");
asm volatile("nop");
digitalWriteFast(WLAN_SCK, LOW);
if (digitalReadFast(WLAN_MISO)) {
receivedData |= 1;
}
asm volatile("nop");
asm volatile("nop");
}
return(receivedData);
}
// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_YEncoderBSet = digitalReadFast(c_YEncoderPinB); // read the input pin
_YEncoderISet = digitalReadFast(c_YEncoderPinI);
_YMinPinSet = digitalReadFast(Y_MIN_PIN);
_YMaxPinSet = digitalReadFast(Y_MAX_PIN);
// and adjust counter + if A leads B
#ifdef RightEncoderIsReversed
_YEncoderTicks -= _YEncoderBSet ? -1 : +1;
if (_YEncoderISet == 0)
_YEncoderIndex -= _YEncoderBSet ? -1 : +1;
#else
_YEncoderTicks += _YEncoderBSet ? -1 : +1;
#endif
}
byte PS2Pad::gamepad_spi(byte send_data) {
byte recv_data = 0;
for(byte i = 0; i < 8; i++) {
digitalWriteFast(CLK_PIN, LOW);
if(send_data & (1 << i)) {
digitalWriteFast(CMD_PIN, HIGH);
} else {
digitalWriteFast(CMD_PIN, LOW);
}
delayMicroseconds(CTRL_CLK);
digitalWriteFast(CLK_PIN, HIGH);
if(digitalReadFast(DAT_PIN)) {
recv_data |= (1 << i);
}
delayMicroseconds(CTRL_CLK);
}
digitalWriteFast(CLK_PIN, HIGH);
delayMicroseconds(CTRL_BYTE_DELAY);
return recv_data;
}
uint8_t PJON::read_byte() {
uint8_t byte_value = B00000000;
/* Delay until the center of the first bit */
delayMicroseconds(BIT_WIDTH / 2);
for(uint8_t i = 0; i < 7; i++) {
/* Read in the center of the n one */
byte_value += digitalReadFast(_input_pin) << i;
/* Delay until the center of the next one */
delayMicroseconds(BIT_WIDTH);
}
/* Read in the center of the last one */
byte_value += digitalReadFast(_input_pin) << 7;
/* Delay until the end of the bit */
delayMicroseconds(BIT_WIDTH / 2);
return byte_value;
}
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
//_XCsens=analogRead(A0);
_XEncoderBSet = digitalReadFast(c_XEncoderPinB); // read the input pin
_XEncoderISet = digitalReadFast(c_XEncoderPinI);
_XMinPinSet = digitalReadFast(X_MIN_PIN);
_XMaxPinSet = digitalReadFast(X_MAX_PIN);
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
_XEncoderTicks -= _XEncoderBSet ? -1 : +1;
if (_XEncoderISet == 0)
_XEncoderIndex -= _XEncoderBSet ? -1 : +1;
#else
_XEncoderTicks += _XEncoderBSet ? -1 : +1;
#endif
}
uint8_t PJON::read_byte() {
uint8_t byte_value = B00000000;
delayMicroseconds(BIT_WIDTH / 2);
for (uint8_t i = 0; i < 8; i++) {
byte_value += digitalReadFast(_input_pin) << i;
delayMicroseconds(BIT_WIDTH);
}
return byte_value;
}
void pollEncoderSW(){
if(digitalReadFast(ENC_SW)){
ENCODER0.millis_up = millis();
ENCODER0.flag = 1;
} else {
ENCODER0.millis_down = millis();
ENCODER0.flag = 0;
}
}
static inline int digitalReadSafe(uint8_t pin) {
if(!__builtin_constant_p(pin)) {
return digitalRead(pin);
}
else {
if(!DIGITALIO_NO_MIX_ANALOGWRITE)
noAnalogWrite(pin);
return digitalReadFast(pin);
}
}
unsigned char hexbright::read_accelerometer(unsigned char acc_reg) {
if (!digitalReadFast(DPIN_ACC_INT)) {
Wire.beginTransmission(ACC_ADDRESS);
Wire.write(acc_reg);
Wire.endTransmission(false); // End, but do not stop!
Wire.requestFrom(ACC_ADDRESS, 1);
return Wire.read();
}
return 0;
}
int PJON::receive_byte() {
pinModeFast(_input_pin, INPUT);
digitalWriteFast(_input_pin, LOW);
unsigned long time = micros();
while (digitalReadFast(_input_pin) && micros() - time <= BIT_SPACER);
time = micros() - time;
if(time >= ACCEPTANCE && !this->syncronization_bit())
return (int)this->read_byte();
return FAIL;
}
/* Measures the length (in microseconds) of a pulse on the pin; state is HIGH
* or LOW, the type of pulse to measure. Works on pulses from 2-3 microseconds
* to 3 minutes in length, but must be called at least a few dozen microseconds
* before the start of the pulse. */
uint32_t pulseIn( uint32_t pin, uint32_t state, uint32_t timeout )
{
// cache the port and bit of the pin in order to speed up the
// pulse width measuring loop and achieve finer resolution. calling
// digitalRead() instead yields much coarser resolution.
const PinDescription* p = &g_aPinMap[pin];
uint32_t width = 0; // keep initialization out of time critical area
// convert the timeout from microseconds to a number of times through
// the initial loop; it takes 22 clock cycles per iteration.
uint32_t numloops = 0;
uint32_t maxloops = microsecondsToClockCycles(timeout) / 22;
// wait for any previous pulse to end
while ( digitalReadFast(p) == state)
{
if (numloops++ == maxloops)
return 0;
}
// wait for the pulse to start
while (digitalReadFast(p) != state)
if (numloops++ == maxloops)
return 0;
// wait for the pulse to stop
while (digitalReadFast(p) == state) {
if (numloops++ == maxloops)
return 0;
width++;
}
// convert the reading to microseconds. The loop has been determined
// to be 52 clock cycles long and have about 16 clocks between the edge
// and the start of the loop. There will be some error introduced by
// the interrupt handlers.
return clockCyclesToMicroseconds(width * 52 + 16);
}
uint16_t PJON::receive_byte() {
/* Initialize the pin and set it to LOW to reduce interference */
pullDownFast(_input_pin);
uint32_t time = micros();
/* Do nothing until the pin goes LOW or passed more time than BIT_SPACER duration */
while(digitalReadFast(_input_pin) && (uint32_t)(micros() - time) <= BIT_SPACER);
/* Save how much time passed */
time = micros() - time;
/* is for sure equal or less than BIT_SPACER, and if is more than ACCEPTANCE
(a minimum HIGH duration) and what is coming after is a LOW bit
probably a byte is coming so try to receive it. */
if(time >= ACCEPTANCE && !this->syncronization_bit())
return (uint8_t)this->read_byte();
return FAIL;
}
unsigned char iBoardRF24::SPI_RW(unsigned char byte) {
unsigned char i;
for(i=0;i<8;i++) {
if(byte&0x80) {
digitalWriteFast(mosi_pin, 1);
} else {
digitalWriteFast(mosi_pin, 0);
}
digitalWriteFast(sck_pin, 1);
byte <<= 1;
if(digitalReadFast(miso_pin) == 1) {
byte |= 1;
}
digitalWriteFast(sck_pin, 0);
}
return (byte);
}
void hexbright::read_button() {
last_button_on = button_on;
button_on = digitalReadFast(DPIN_RLED_SW);
if(button_on) {
if(!last_button_on) {
time_last_pressed=millis();
#if (DEBUG==DEBUG_BUTTON)
Serial.println("Button just pressed");
Serial.print("Time spent released (ms): ");
Serial.println(time_last_pressed-time_last_released);
#endif
}
} else { // button is off
if(last_button_on) {
time_last_released=millis();
#if (DEBUG==DEBUG_BUTTON)
Serial.println("Button just released");
Serial.print("Time spent pressed (ms): ");
Serial.println(time_last_released-time_last_pressed);
#endif
}
}
}
void hexbright::read_button() {
/*button_state = button_state << 1; // make space for the new value
button_state = button_state | digitalReadFast(DPIN_RLED_SW); // add the new value
button_state = button_state & BUTTON_FILTER; // remove excess values */
// Doing the three commands above on one line saves 2 bytes. We'll take it!
button_state = ((button_state<<1) | digitalReadFast(DPIN_RLED_SW)) & BUTTON_FILTER;
if(BUTTON_JUST_ON(button_state)) {
time_last_pressed=millis();
#if (DEBUG==DEBUG_BUTTON)
Serial.println("Button just pressed");
Serial.print("Time spent released (ms): ");
Serial.println(time_last_pressed-time_last_released);
#endif
} else if(BUTTON_JUST_OFF(button_state)) {
time_last_released=millis();
#if (DEBUG==DEBUG_BUTTON)
Serial.println("Button just released");
Serial.print("Time spent pressed (ms): ");
Serial.println(time_last_released-time_last_pressed);
#endif
}
}
byte SPIPump(byte data) {
#if(USE_HARDWARE_SPI)
return(SPI.transfer(data));
#else
byte receivedData=0;
for (int8_t i=7; i>=0; i--) {
receivedData <<= 1;
if (data & (1<<i)) {
digitalWriteFast(MOSI, HIGH);
}
else {
digitalWriteFast(MOSI, LOW);
}
digitalWriteFast(SCK, HIGH);
asm volatile("nop");
asm volatile("nop");
digitalWriteFast(SCK, LOW);
if (digitalReadFast(MISO)) {
receivedData |= 1;
}
asm volatile("nop");
asm volatile("nop");
}
return(receivedData);
#endif
}
uint8_t PJON::syncronization_bit() {
delayMicroseconds((BIT_WIDTH / 2) - READ_DELAY);
uint8_t bit_value = digitalReadFast(_input_pin);
delayMicroseconds(BIT_WIDTH / 2);
return bit_value;
}
// Taken and modified from: http://www.mkesc.co.uk/ise.pdf
// This fires when a pulse is generated by edge detector circuit on external interrupt 0
void Encoder::quadratureDecoderISR(void)
{
encoder->encoderValues <<= 2;
encoder->encoderValues |= ((digitalReadFast(Utility::PIN_EncoderChannelA) << 1) | digitalReadFast(Utility::PIN_EncoderChannelB));
encoder->currentAngle += encoderLookup[encoder->encoderValues & 0x0F] * DBL_DegreesPerPulse;
}
/* Handles encoder interrupts for finger 5 */
void Hand::Encoder5() {
Hand::finger[5].position -= 1 - ((digitalReadFast(encoderAPins[5]) ^ digitalReadFast(encoderBPins[5])) << 1);
}
/* Handles encoder interrupts for finger 0 */
void Hand::Encoder0() {
Hand::finger[0].position += 1 - ((digitalReadFast(encoderAPins[0]) ^ digitalReadFast(encoderBPins[0])) << 1);
}
int saturn_read() {
int retval = 0;
/*
* S0 S1 D0 d1 d2 d3
* Off Off Z Y X R
* On Off B C A St
* Off On Up Dn Lt Rt
* On On - - - L
*/
// Reading L
digitalWriteFast(S0, HIGH);
digitalWriteFast(S1, HIGH);
delayMicroseconds(DELAY);
retval |= (!digitalReadFast(D3) << 12); // L
// Reading Z, Y, X and R
digitalWriteFast(S0, LOW);
digitalWriteFast(S1, LOW);
delayMicroseconds(DELAY);
retval |= (!digitalReadFast(D0) << 0); // Z
retval |= (!digitalReadFast(D1) << 1); // Y
retval |= (!digitalReadFast(D2) << 2); // X
retval |= (!digitalReadFast(D3) << 3); // R
// Reading B, C, A and Start
digitalWriteFast(S0, HIGH);
digitalWriteFast(S1, LOW);
delayMicroseconds(DELAY);
retval |= (!digitalReadFast(D0) << 4); // B
retval |= (!digitalReadFast(D1) << 5); // C
retval |= (!digitalReadFast(D2) << 6); // A
retval |= (!digitalReadFast(D3) << 7); // Start
// Reading Up, Down, Left, Right
digitalWriteFast(S0, LOW);
digitalWriteFast(S1, HIGH);
delayMicroseconds(DELAY);
retval |= (!digitalReadFast(D0) << 8); // Up
retval |= (!digitalReadFast(D1) << 9); // Down
retval |= (!digitalReadFast(D2) << 10); // Left
retval |= (!digitalReadFast(D3) << 11); // Right
return retval;
}
void WatchCore::doInput() {
while (Serial.available()) {
String data = Serial.readStringUntil(':');
/* Try getting the time from serial. */
if (data.endsWith("Time")) {
const time_t DEFAULT_TIME = 1357041600;
time_t t = Serial.parseInt();
if (t > DEFAULT_TIME)
setCurrentTime(t);
}
}
uint32_t lft = 0;
uint32_t rgt = 0;
uint32_t up = 0;
uint32_t dwn = 0;
uint8_t lftNew = 0;
uint8_t rgtNew = 0;
uint8_t upNew = 0;
uint8_t dwnNew = 0;
/* Poll the hall effect sensors for any changes over 8 milliseconds. */
for(uint32_t time = millis() + 4; time > millis();) {
lftNew = digitalReadFast(TRACKBALL_LFT);
rgtNew = digitalReadFast(TRACKBALL_RGT);
upNew = digitalReadFast(TRACKBALL_UP);
dwnNew = digitalReadFast(TRACKBALL_DWN);
lft += lftLast != lftNew;
rgt += rgtLast != rgtNew;
up += upLast != upNew;
dwn += dwnLast != dwnNew;
lftLast = lftNew;
rgtLast = rgtNew;
upLast = upNew;
dwnLast = dwnNew;
}
/* LOW is pressed. */
if (digitalRead(TRACKBALL_BTN) == LOW) {
if (buttonTime == 0)
buttonTime = now();
} else {
if(buttonTime != 0) {
/* If the buzzer is enabled all pressing a button does is stop it. */
if (buzzer > now()) {
buzzer = 0;
} else if (currentMenu != nullptr) {
if (currentMenu[currentMenuItem].callback != nullptr && currentMenu[currentMenuItem].callback(modes[currentMode], *this))
currentMenu = nullptr;
else if (currentMenu[currentMenuItem].subMenu != nullptr)
openMenu(currentMenu[currentMenuItem].subMenu);
} else {
modes[currentMode]->buttonPress(now() - buttonTime);
}
}
buttonTime = 0;
}
if (currentMenu != nullptr) {
currentMenuItem += dwn - up;
if (currentMenuItem < 0)
currentMenuItem = currentMenuLength - 1;
else if (currentMenuItem >= currentMenuLength)
currentMenuItem = 0;
if (lft > 0)
popMenu();
else if (rgt > 0 && currentMenu[currentMenuItem].subMenu != nullptr)
openMenu(currentMenu[currentMenuItem].subMenu);
} else {
if (lft > 0)
modes[currentMode]->left(lft);
if (rgt > 0)
modes[currentMode]->right(rgt);
if (up > 0)
modes[currentMode]->up(up);
if (dwn > 0)
modes[currentMode]->down(dwn);
}
}
// update functions; meant to be attached to ISR
void pollEncoder(){
digitalReadFast(ENC_B) ? ENCODER0.pos++ : ENCODER0.pos--;
}
void scanSerialPort()
{
char incomingChar;
uint8_t linePointer = 0;
char tmpstr[100];
uint16_t heartBeatTimer = 0;
uint16_t potentiometerTimer = 0;
while (true)
{
// Use this to toggle heartbeat LED when not receiving characters
while (Serial.available() <= 0)
{
if (++heartBeatTimer >= 65535)
{
heartBeatTimer = 0;
digitalWriteFast(HEARTBEAT_LED_PIN, !digitalReadFast(HEARTBEAT_LED_PIN));
}
// Update the potentiometer only if PID isn't on, since it will be updated in the block below
if (++potentiometerTimer >= 5000)
{
if (!isPidEnabled)
{
updatePotentiometerAngle();
}
potentiometerTimer = 0;
}
if (isPidCalculationNeeded)
{
updatePotentiometerAngle();
executePidCalculation();
isPidCalculationNeeded = false;
}
}
incomingChar = (char)Serial.read();
if (incomingChar)
{
if (incomingChar == '\n') // End of input
{
lineBuffer[linePointer] = 0;
linePointer = 0;
sprintf(tmpstr, "> %s", lineBuffer);
Serial.println(tmpstr);
processLine(lineBuffer);
}
else if (incomingChar == '\r') // Discard the carriage return
{
}
else // Store any other characters in the buffer
{
lineBuffer[linePointer++] = incomingChar;
lineBuffer[linePointer] = 0;
if (linePointer >= INT_LINE_SIZE_MAX - 1)
{
linePointer = INT_LINE_SIZE_MAX - 1;
lineBuffer[linePointer] = 0;
}
}
}
}
}
int tg16_read(void) {
int retval = 0;
// Data Select HIGH
digitalWriteFast(10, HIGH);
// /OE LOW
digitalWriteFast(11, LOW);
for(int i = 0; i < 2; i++) {
// /OE LOW
digitalWriteFast(11, LOW);
delayMicroseconds(1);
// If four directions are low, then it's an Avenue6 Pad
if(!digitalReadFast(5) && !digitalReadFast(7) && !digitalReadFast(8) && !digitalReadFast(9)) {
// Data Select LOW
digitalWriteFast(10, LOW);
delayMicroseconds(1);
retval |= (!digitalReadFast(5) << 8); // III
retval |= (!digitalReadFast(7) << 9); // IV
retval |= (!digitalReadFast(8) << 10); // V
retval |= (!digitalReadFast(9) << 11); // VI
} else {
// Normal pad reading
retval |= (!digitalReadFast(5) << 0); // UP
retval |= (!digitalReadFast(7) << 1); // RIGHT
retval |= (!digitalReadFast(8) << 2); // DOWN
retval |= (!digitalReadFast(9) << 3); // LEFT
// Data Select LOW
digitalWriteFast(10, LOW);
delayMicroseconds(1);
retval |= (!digitalReadFast(5) << 4); // I
retval |= (!digitalReadFast(7) << 5); // II
retval |= (!digitalReadFast(8) << 6); // SELECT
retval |= (!digitalReadFast(9) << 7); // RUN
}
// Data Select HIGH
digitalWriteFast(10, HIGH);
// /OE HIGH
digitalWriteFast(11, HIGH);
}
return retval;
}
void loop(void) {
left = !digitalReadFast(PIN_LEFT);
right = !digitalReadFast(PIN_RIGHT);
up = !digitalReadFast(PIN_UP);
down = !digitalReadFast(PIN_DOWN);
sqre = !digitalReadFast(PIN_SQUARE);
triangle = !digitalReadFast(PIN_TRIANGLE);
circle = !digitalReadFast(PIN_CIRCLE);
cross = !digitalReadFast(PIN_CROSS);
select = !digitalReadFast(PIN_SELECT);
start = !digitalReadFast(PIN_START);
l1 = !digitalReadFast(PIN_L1);
r1 = !digitalReadFast(PIN_R1);
pspad_set_pad_state(left, right, up, down, sqre, triangle, circle,
cross, select, start, l1, l2, r1, r2, l3, r3, lx, ly, rx, ry);
}
void ControlLoopSTEPPER::update() {
// -------------------------------
//
// for timing / testing:
//
// -------------------------------
digitalWriteFast(TP_INT_CLOCK,HIGH);
#ifdef INVERTED_PINS
digitalWriteFast(pin_STEP_A, HIGH);
#else
digitalWriteFast(pin_STEP_A, LOW);
#endif
// -------------------------------
//
// do the bresenham's:
//
// -------------------------------
if (A_theD > 0) {
// do a step!
#ifdef INVERTED_PINS
digitalWriteFast(pin_STEP_A, LOW);
#else
digitalWriteFast(pin_STEP_A, HIGH);
#endif
A_theD += A_twoDy_twoDx;
/* ---- Keep track of position ---- */
if (currentDirection) {
currentPosition ++;
} else {
currentPosition --;
}
/* ---- check for home sensor ---- */
if (digitalReadFast(HOME_SENSOR_SIGNAL) != homeSensorState) {
if (!homeSensorState) {
// sensor going high so record position
homeSensorState = true;
homeSensorPosition = currentPosition;
} else {
homeSensorState = false;
}
}
} else {
A_theD += A_twoDy;
}
// -------------------------------
//
// Prepare for next time round
//
// -------------------------------
// are we at the end of this subframe?
currentStep--;
if (currentStep == 0) {
// yes, so get next deltas and calc Bres coefficients
// for testing:
digitalWriteFast(TP_GETNEXT, HIGH);
// channel A:
nextPosition = getNextPosition();
nextDelta_A = nextPosition - currentPosition;
if (nextDelta_A < 0) {
// direction positive
digitalWrite(pin_DIR_A,HIGH);
currentDirection = false;
nextDelta_A = -nextDelta_A;
}
else {
// direction negative
digitalWrite(pin_DIR_A,LOW);
currentDirection = true;
}
A_twoDy = 2 * nextDelta_A;
A_twoDy_twoDx = A_twoDy - (INNERS_PER_OUTER * 2);
A_theD = A_twoDy_twoDx + INNERS_PER_OUTER;
// tidy up:
nextDelta_A = 0;
currentStep = INNERS_PER_OUTER;
digitalWriteFast(TP_GETNEXT, LOW);
}
// for timing testing:
digitalWriteFast(TP_INT_CLOCK,LOW);
}
