eka/hxbot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
master
hxbot/REflex/REflex.ino

644 lines
15 KiB
C++
Raw Permalink Blame History

/////////////////////////////////////////
// HXbot REflex firmware /
// EoF 2016 EoF@itphx.ru /
/////////////////////////////////////////
#include <PowerFunctions.h>
#include <Ultrasonic.h>
#include <Wire.h>
#include <Servo.h>
// 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;
}
Reference in New Issue View Git Blame Copy Permalink