///////////////////////////////////////// // HXbot REflex firmware / // EoF 2016 EoF@itphx.ru / ///////////////////////////////////////// #include #include #include #include // DEBUG #define DEBUG 1 // DEFINE #define SLAVE_ADDRESS 0x04 #define XOR_SEQ 0xFF #define EXT_COM 0xAA //#define LED_PIN 13 #define IRE_PIN 2 #define IRR_PIN 3 #define SV_PIN 13 // vertical #define SV_CENTER 150 // 0 - up (min. 60), 180 - down (max. 170) #define SH_PIN 12 //horisontal #define SH_CENTER 90 // 0 - right, 180 - left #define SERVO_DELAY 10 // time interval between servo moves #define ULTRASONIC_PIN 6 #define DRIVE_CHANNEL 2 #define TIMEOUT 5000 #define HEAD_V_FLAG 0x31 #define HEAD_H_FLAG 0x30 // Front and rear servos #define SF_PIN 10 // front servo #define SF_CENTER 96 #define SR_PIN 11 // rear servo #define SR_CENTER 93 #define FR_STEP 2 // IR distance sensors #define IRD_FRONT_PIN 0 #define IRD_F_MAX 3.0 #define IRA_FRONT_PIN 1 #define IRA_F_MIN 1.0 #define IRD_REAR_PIN 2 #define IRD_R_MAX 3.0 #define IRA_REAR_PIN 3 #define IRA_R_MIN 1.0 #define IR_REPEAT_COUNT 3 // COMMANDS // Util #define COM_PING 0x01 #define COM_GET_TEMP 0x02 #define COM_GET_DISTANCE 0x03 // Stop #define STOP 0x0F // Move #define MOVE_FLAG 0x04 #define MOVE_FLOAT 0x10 #define MOVE_FWD1 0x11 #define MOVE_FWD2 0x12 #define MOVE_FWD3 0x13 #define MOVE_FWD4 0x14 #define MOVE_FWD5 0x15 #define MOVE_FWD6 0x16 #define MOVE_FWD7 0x17 #define MOVE_BREAK 0x18 #define MOVE_REV7 0x19 #define MOVE_REV6 0x1A #define MOVE_REV5 0x1B #define MOVE_REV4 0x1C #define MOVE_REV3 0x1D #define MOVE_REV2 0x1E #define MOVE_REV1 0x1F // Steering #define STEER_FLAG 0x05 #define STEER_CENTER 0x20 #define STEER_LEFT1 0x21 #define STEER_LEFT2 0x22 #define STEER_LEFT3 0x23 #define STEER_LEFT4 0x24 #define STEER_LEFT5 0x25 #define STEER_LEFT6 0x26 #define STEER_LEFT7 0x27 #define STEER_FIX_CENTER 0x28 #define STEER_RIGHT7 0x29 #define STEER_RIGHT6 0x2A #define STEER_RIGHT5 0x2B #define STEER_RIGHT4 0x2C #define STEER_RIGHT3 0x2D #define STEER_RIGHT2 0x2E #define STEER_RIGHT1 0x2F // Response #define OK_RSP 0x00 #define NO_RSP 0xFF #define ERR_RSP 0x01 #define BLK_RSP 0x02 #define CSE_RSP 0x03 #define IOE_RSP 0x04 #define TMO_RSP 0x05 // VAR const float typVbg = 1.13; // 1.0 -- 1.2 float ird_f, ira_f, ird_r, ira_r; boolean ird_ignore = false, ira_ignore = false; byte cmd = 0; byte flg = 0; byte ext = 0; byte autoresponse = 0; long usDistance = 0; unsigned long lastCommandTime = 0; boolean isMove = false; boolean frBlock = false; PowerFunctions pfDrive(IRE_PIN, DRIVE_CHANNEL); Ultrasonic usSensor(ULTRASONIC_PIN); Servo servV, servH, servF, servR; byte destV, destH; unsigned long lastServoMove = 0; // SETUP void setup() { if (DEBUG) { Serial.println("GO!"); Serial.begin(9600); } //pinMode(LED_PIN, OUTPUT); // Initialize i2c as slave Wire.begin(SLAVE_ADDRESS); // Define callbacks for i2c communication Wire.onReceive(receiveData); Wire.onRequest(answer); analogReference(DEFAULT); destH = SH_CENTER; destV = SV_CENTER; if (DEBUG) { Serial.println("Setup complete"); } } // MAIN LOOP void loop() { // Timeout protection if ((lastCommandTime + TIMEOUT < millis()) && isMove) { pfDrive.combo_pwm(PWM_BRK, PWM_BRK); servV.detach(); servH.detach(); isMove = false; autoresponse = TMO_RSP; } moveServo(); //usDistance = usSensor.MeasureInCentimeters(); if (DEBUG) { ird_f = analogRead(IRD_FRONT_PIN) * readvcc() / 1024; ira_f = analogRead(IRA_FRONT_PIN) * readvcc() / 1024; ird_r = analogRead(IRD_REAR_PIN) * readvcc() / 1024; ira_r = analogRead(IRA_REAR_PIN) * readvcc() / 1024; Serial.print("VCC: "); Serial.println(readvcc()); Serial.print("IRD_F: "); Serial.println(ird_f); Serial.print("IRA_F: "); Serial.println(ira_f); Serial.print("IRD_R: "); Serial.println(ird_r); Serial.print("IRA_R: "); Serial.println(ira_r); Serial.println("------"); delay(1000); } //delay(100); } void moveResponse(byte rsp = OK_RSP) { lastCommandTime = millis(); isMove = true; autoresponse = rsp; } // Callback for received data void receiveData(int byteCount) { while(Wire.available()) { // Get command cmd = Wire.read(); if (cmd == EXT_COM && byteCount == 3) { flg = 0x00; ext = 0x00; if (Wire.available()) ext = Wire.read(); if (Wire.available()) flg = Wire.read(); } else { // Cleanup I2C bus while(Wire.available()) { ext = Wire.read(); } } switch (cmd) { case COM_PING: autoresponse = OK_RSP; break; case STOP: pfDrive.combo_pwm(PWM_BRK, PWM_BRK); moveResponse(); break; case COM_GET_TEMP: autoresponse = getInternalTemp(); break; case COM_GET_DISTANCE: usDistance = usSensor.MeasureInCentimeters(); if (usDistance >= 255) { autoresponse = NO_RSP; } else { autoresponse = usDistance; } break; case EXT_COM: switch (flg) { case HEAD_V_FLAG: case HEAD_H_FLAG: headControl(flg, ext); break; case MOVE_FLAG: case STEER_FLAG: moveControl(flg, ext); break; } break; default: autoresponse = ERR_RSP; break; } } } void headControl(byte flg, byte val) { if (! servV.attached()) servV.attach(SV_PIN); if (! servH.attached()) servH.attach(SH_PIN); switch (flg) { case HEAD_V_FLAG: destV = val; break; case HEAD_H_FLAG: destH = val; break; } moveResponse(); } void moveServo() { byte curV, curH; if (! isMove) return; if (lastServoMove + SERVO_DELAY < millis()) { curV = servV.read(); curH = servH.read(); if (destV < curV) servV.write(curV - 1); if (destV > curV) servV.write(curV + 1); if (destH < curH) servH.write(curH - 1); if (destH > curH) servH.write(curH + 1); lastServoMove = millis(); } } // Callback for sending data void answer() { Wire.write(autoresponse); } boolean ird_f_ok() { if (! ird_ignore) { ird_f = 0.0; for (byte i = 0; i < IR_REPEAT_COUNT; i++) { ird_f = ird_f + analogRead(IRD_FRONT_PIN) * readvcc() / 1024; } ird_f = ird_f / IR_REPEAT_COUNT; if (ird_f <= IRD_F_MAX) { return true; } else { return false; } } else return true; } boolean ird_r_ok() { if (! ird_ignore) { ird_r = 0.0; for (byte i = 0; i < IR_REPEAT_COUNT; i++) { ird_r = ird_r + analogRead(IRD_REAR_PIN) * readvcc() / 1024; } ird_r = ird_r / IR_REPEAT_COUNT; if (ird_r <= IRD_R_MAX) { return true; } else { return false; } } else return true; } boolean ira_f_ok() { if (! ira_ignore) { ira_f = 0.0; for (byte i = 0; i < IR_REPEAT_COUNT; i++) { ira_f = ira_f + analogRead(IRA_FRONT_PIN) * readvcc() / 1024; } ira_f = ira_f / IR_REPEAT_COUNT; if (ira_f >= IRA_F_MIN) { return true; } else { return false; } } else return true; } boolean ira_r_ok() { if (! ira_ignore) { ira_r = 0.0; for (byte i = 0; i < IR_REPEAT_COUNT; i++) { ira_r = ira_r + analogRead(IRA_REAR_PIN) * readvcc() / 1024; } ira_r = ira_r / IR_REPEAT_COUNT; if (ira_r >= IRA_R_MIN) { return true; } else { return false; } } else return true; } void stopMove() { pfDrive.single_pwm(RED, PWM_BRK); pfDrive.single_pwm(RED, PWM_BRK); pfDrive.single_pwm(RED, PWM_BRK); } void moveControl(byte flg, byte cmd) { if (flg == MOVE_FLAG) { if (cmd >= MOVE_FWD1 && cmd <= MOVE_FWD7) { if (! ira_f_ok()) { stopMove(); moveResponse(BLK_RSP); return; } if (! ird_f_ok()) { stopMove(); moveResponse(BLK_RSP); return; } } if (cmd >= MOVE_REV7 && cmd <= MOVE_REV1) { if (! ira_r_ok()) { stopMove(); moveResponse(BLK_RSP); return; } if (! ird_r_ok()) { stopMove(); moveResponse(BLK_RSP); return; } } } switch (cmd) { // Moving case MOVE_FLOAT: pfDrive.single_pwm(RED, PWM_FLT); break; case MOVE_FWD1: pfDrive.single_pwm(RED, PWM_FWD1); break; case MOVE_FWD2: pfDrive.single_pwm(RED, PWM_FWD2); break; case MOVE_FWD3: pfDrive.single_pwm(RED, PWM_FWD3); break; case MOVE_FWD4: pfDrive.single_pwm(RED, PWM_FWD4); break; case MOVE_FWD5: pfDrive.single_pwm(RED, PWM_FWD5); break; case MOVE_FWD6: pfDrive.single_pwm(RED, PWM_FWD6); break; case MOVE_FWD7: pfDrive.single_pwm(RED, PWM_FWD7); break; case MOVE_BREAK: pfDrive.single_pwm(RED, PWM_BRK); break; case MOVE_REV1: pfDrive.single_pwm(RED, PWM_REV1); break; case MOVE_REV2: pfDrive.single_pwm(RED, PWM_REV2); break; case MOVE_REV3: pfDrive.single_pwm(RED, PWM_REV3); break; case MOVE_REV4: pfDrive.single_pwm(RED, PWM_REV4); break; case MOVE_REV5: pfDrive.single_pwm(RED, PWM_REV5); break; case MOVE_REV6: pfDrive.single_pwm(RED, PWM_REV6); break; case MOVE_REV7: pfDrive.single_pwm(RED, PWM_REV7); break; // Steering case STEER_CENTER: pfDrive.single_pwm(BLUE, PWM_FLT); frControl(cmd); break; case STEER_LEFT1: pfDrive.single_pwm(BLUE, PWM_FWD1); frControl(cmd); break; case STEER_LEFT2: pfDrive.single_pwm(BLUE, PWM_FWD2); frControl(cmd); break; case STEER_LEFT3: pfDrive.single_pwm(BLUE, PWM_FWD3); frControl(cmd); break; case STEER_LEFT4: pfDrive.single_pwm(BLUE, PWM_FWD4); frControl(cmd); break; case STEER_LEFT5: pfDrive.single_pwm(BLUE, PWM_FWD5); frControl(cmd); break; case STEER_LEFT6: pfDrive.single_pwm(BLUE, PWM_FWD6); frControl(cmd); break; case STEER_LEFT7: pfDrive.single_pwm(BLUE, PWM_FWD7); frControl(cmd); break; case STEER_FIX_CENTER: pfDrive.single_pwm(BLUE, PWM_BRK); frControl(cmd); break; case STEER_RIGHT1: pfDrive.single_pwm(BLUE, PWM_REV1); frControl(cmd); break; case STEER_RIGHT2: pfDrive.single_pwm(BLUE, PWM_REV2); frControl(cmd); break; case STEER_RIGHT3: pfDrive.single_pwm(BLUE, PWM_REV3); frControl(cmd); break; case STEER_RIGHT4: pfDrive.single_pwm(BLUE, PWM_REV4); frControl(cmd); break; case STEER_RIGHT5: pfDrive.single_pwm(BLUE, PWM_REV5); frControl(cmd); break; case STEER_RIGHT6: pfDrive.single_pwm(BLUE, PWM_REV6); frControl(cmd); break; case STEER_RIGHT7: pfDrive.single_pwm(BLUE, PWM_REV7); frControl(cmd); break; default: moveResponse(NO_RSP); return; break; } // Send response moveResponse(); } void frControl(byte cmd) { if (! frBlock) { if (! servF.attached()) servF.attach(SF_PIN); if (! servR.attached()) servR.attach(SR_PIN); switch (cmd) { case STEER_CENTER: servF.write(SF_CENTER); servR.write(SR_CENTER); break; case STEER_LEFT1: servF.write(SF_CENTER - FR_STEP * 1); servR.write(SR_CENTER + FR_STEP * 1); break; case STEER_LEFT2: servF.write(SF_CENTER - FR_STEP * 2); servR.write(SR_CENTER + FR_STEP * 2); break; case STEER_LEFT3: servF.write(SF_CENTER - FR_STEP * 3); servR.write(SR_CENTER + FR_STEP * 3); break; case STEER_LEFT4: servF.write(SF_CENTER - FR_STEP * 4); servR.write(SR_CENTER + FR_STEP * 4); break; case STEER_LEFT5: servF.write(SF_CENTER - FR_STEP * 5); servR.write(SR_CENTER + FR_STEP * 5); break; case STEER_LEFT6: servF.write(SF_CENTER - FR_STEP * 6); servR.write(SR_CENTER + FR_STEP * 6); break; case STEER_LEFT7: servF.write(SF_CENTER - FR_STEP * 7); servR.write(SR_CENTER + FR_STEP * 7); break; case STEER_FIX_CENTER: servF.write(SF_CENTER); servR.write(SR_CENTER); break; case STEER_RIGHT1: servF.write(SF_CENTER + FR_STEP * 1); servR.write(SR_CENTER - FR_STEP * 1); break; case STEER_RIGHT2: servF.write(SF_CENTER + FR_STEP * 2); servR.write(SR_CENTER - FR_STEP * 2); break; case STEER_RIGHT3: servF.write(SF_CENTER + FR_STEP * 3); servR.write(SR_CENTER - FR_STEP * 3); break; case STEER_RIGHT4: servF.write(SF_CENTER + FR_STEP * 4); servR.write(SR_CENTER - FR_STEP * 4); break; case STEER_RIGHT5: servF.write(SF_CENTER + FR_STEP * 5); servR.write(SR_CENTER - FR_STEP * 5); break; case STEER_RIGHT6: servF.write(SF_CENTER + FR_STEP * 6); servR.write(SR_CENTER - FR_STEP * 6); break; case STEER_RIGHT7: servF.write(SF_CENTER + FR_STEP * 7); servR.write(SR_CENTER - FR_STEP * 7); break; default: servF.write(SF_CENTER); servR.write(SR_CENTER); break; } } } // Get the internal temperature of the arduino double getInternalTemp(void) { unsigned int wADC; double t; ADMUX = (_BV(REFS1) | _BV(REFS0) | _BV(MUX3)); ADCSRA |= _BV(ADEN); // enable the ADC delay(20); // wait for voltages to become stable. ADCSRA |= _BV(ADSC); // Start the ADC while (bit_is_set(ADCSRA,ADSC)); wADC = ADCW; t = (wADC - 324.31 ) / 1.22; return (t); } float readvcc() { float tmp = 0.0; // Read 1.1V reference against Avcc // set the reference to vcc and the measurement to the internal 1.1V reference #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) ADMUX = _BV(MUX5) | _BV(MUX0); #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) ADMUX = _BV(MUX3) | _BV(MUX2); #else // works on an Arduino 168 or 328 ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); #endif delay(3); // Wait for Vref to settle ADCSRA |= _BV(ADSC); // Start conversion while (bit_is_set(ADCSRA,ADSC)); // measuring uint8_t low = ADCL; // must read ADCL first - it then locks ADCH uint8_t high = ADCH; // unlocks both tmp = (high<<8) | low; tmp = (typVbg * 1023.0) / tmp; return tmp; }