// Tianchen TC-G158 configuration (using original board connected to Arduino)
// (Arduino Mega)
/*
STC uC pinout:
04 02 32 30 01- 09-TILT 17- 25-MR.S
05 06 03 01 31 29 02-BUZZ 10- 18-MOW.M 26-MUX.S1
07 08 28 27 03- 11- 19-MR.D 27-ML.S
09 10 TOP 26 25 04- 12- 20-MOW.S 28-L.1234
11 12 24 23 05- 13-MR.C 21-BUMP.L 29-MUX.Z (0=bat, 1=rain, 2=pow, 3=pow)
13 15 17 19 22 21 06-RC 14-ML.C 22-BUMP.R 30-ML.D
14 16 18 20 07- 15- 23-MUX.S0 31-IR
08- 16- 24- 32-
*/
// ------ configuration --------------------------------------------------
// ------ features
const char driveBiDir = 0; // drive bidirectional (1) (forward/backward) or make turns only (0)?
const char useBumper = 1; // has bumpers?
const char useCompass = 0; // use compass sensor?
const char useAccel = 1; // use ultra sonic sensor?
const char useSonar = 1; // use acceleration sensor?
const char useTilt = 1; // has tilt sensor? (required for TC-G158 board)
const char useButton = 1; // has digital ON/OFF button?
const char usePerimeter = 1; // use perimeter?
// ------ pins
#define pinMotorLeftPWM 4 // left motor pins
#define pinMotorLeftDir 31
#define pinMotorLeftSense A1
#define pinMotorRightPWM 3 // right motor pins
#define pinMotorRightDir 33
#define pinMotorRightSense A0
#define pinMotorMow 2 // mower motor pins
#define pinMotorMowSense A3
#define pinBumperLeft 39 // bumper pins
#define pinBumperRight 37
//#define pinSonarCenterTrigger 24 // ultrasonic sensor pins
//#define pinSonarCenterEcho 22
#define pinSonarRightTrigger 30
#define pinSonarRightEcho 32
#define pinSonarLeftTrigger 34
#define pinSonarLeftEcho 36
//#define pinPerimeterRight A4 // perimeter
//#define pinPerimeterLeft A5
#define pinPerimeterMuxZ A6 // perimeter mux Z (only TC-G158 board)
#define pinLED 13 // LED
#define pinBuzzer 53 // Buzzer
#define pinTilt 35 // Tilt sensor (required for TC-G158 board)
#define pinButton 51 // digital ON/OFF button
//#define pinBatteryVoltage A2 // battery voltage sensor
//#define pinChargeCurrent A8 // charge current sensor
#define pinMuxS0 28 // mux S0 (only TC-G158 board)
#define pinMuxS1 26 // mux S1 (only TC-G158 board)
#define pinMuxZ A7 // mux Z (only TC-G158 board)
// compass (I2C): SCL A5 , SDA A4
// ------- other
#define BAUDRATE 19200 // serial output baud rate
#define MAX_MOTOR_SPEED 127 // motor wheel max PWM (8-bit PWM=255, 10-bit PWM=1023)
#define MAX_MOTOR_CURRENT 30 // motor wheel max current (8-bit ADC:0..255 , 10-bit ADC:0..1023)
#define MAX_MOW_SPEED 63 // motor mower max PWM
#define MAX_MOW_CURRENT 60 // motor mower max current
#define motorRightSenseZero 0 // motor right sense zero point
#define motorLeftSenseZero 0 // motor left sense zero point
#define motorMowSenseZero 0 // motor mower sense zero point
#define chargeSenseZero 0 // charge current sense zero point
#define BATTERY_FAC 4.7 // battery conversion factor
#define MAX_ECHO_TIME 5000 // ultrasonic sensor max echo time
#define MAX_PERIMETER 900 // perimeter threshold
// ------ configuration end -------------------------------------------
void config(){
pinMode(pinMotorLeftPWM, OUTPUT);
pinMode(pinMotorLeftDir, OUTPUT);
pinMode(pinMotorRightPWM, OUTPUT);
pinMode(pinMotorRightDir, OUTPUT);
pinMode(pinMotorMow, OUTPUT);
digitalWrite(pinMotorMow, HIGH);
//pinMode(pinMotorLeftSense, INPUT);
//pinMode(pinMotorRightSense, INPUT);
pinMode(pinMotorMowSense, INPUT);
pinMode(pinLED, OUTPUT);
pinMode(pinBuzzer, OUTPUT);
//pinMode(pinBatteryVoltage, INPUT);
pinMode(pinMuxZ, INPUT);
pinMode(pinPerimeterMuxZ, INPUT);
pinMode(pinMuxS0, OUTPUT);
pinMode(pinMuxS1, OUTPUT);
if (usePerimeter){
//pinMode(pinPerimeterRight, INPUT);
//pinMode(pinPerimeterLeft, INPUT);
}
if (useButton){
pinMode(pinButton, INPUT);
pinMode(pinButton, INPUT_PULLUP);
}
if (useBumper){
pinMode(pinBumperLeft, INPUT);
pinMode(pinBumperLeft, INPUT_PULLUP);
pinMode(pinBumperRight, INPUT);
pinMode(pinBumperRight, INPUT_PULLUP);
}
if (useTilt){
pinMode(pinTilt, INPUT);
pinMode(pinTilt, INPUT_PULLUP);
}
if (useSonar){
//pinMode(pinSonarCenterTrigger, OUTPUT);
//pinMode(pinSonarCenterEcho, INPUT);
pinMode(pinSonarLeftTrigger, OUTPUT);
pinMode(pinSonarLeftEcho, INPUT);
pinMode(pinSonarRightTrigger, OUTPUT);
pinMode(pinSonarRightEcho, INPUT);
}
if (useAccel) initADXL345B();
if (useCompass) initHMC5883L();
}
void mux(int channel){
digitalWrite(pinMuxS0, channel & 1);
digitalWrite(pinMuxS1, channel & 2);
delay(1);
}
int readSensor(char type){
switch (type) {
case SEN_MOTOR_RIGHT: return(analogRead(pinMotorRightSense)-motorRightSenseZero); break;
case SEN_MOTOR_LEFT: return(analogRead(pinMotorLeftSense)-motorLeftSenseZero); break;
case SEN_MOTOR_MOW: return(analogRead(pinMotorMowSense)-motorMowSenseZero); break;
case SEN_PERIM_LEFT: mux(0); return(analogRead(pinPerimeterMuxZ)); break;
case SEN_PERIM_RIGHT: mux(2); return(analogRead(pinPerimeterMuxZ)); break;
case SEN_PERIM_LEFT_EXTRA: mux(3); return(analogRead(pinPerimeterMuxZ)); break;
case SEN_PERIM_RIGHT_EXTRA: mux(1); return(analogRead(pinPerimeterMuxZ)); break;
case SEN_BAT_VOLTAGE: mux(0); return((int)(((double)analogRead(pinMuxZ)) * BATTERY_FAC)); break;
//case SEN_CHG_CURRENT: return(analogRead(pinChargeCurrent)-chargeSenseZero); break;
case SEN_BUMPER_RIGHT: return(digitalRead(pinBumperRight)); break;
case SEN_BUMPER_LEFT: return(digitalRead(pinBumperLeft)); break;
case SEN_BUTTON: return(digitalRead(pinButton)); break;
case SEN_SONAR_RIGHT: return(readHCSR04(pinSonarRightTrigger, pinSonarRightEcho)); break;
case SEN_SONAR_LEFT: return(readHCSR04(pinSonarLeftTrigger, pinSonarLeftEcho)); break;
case SEN_ACCEL: readADXL345B(); break;
case SEN_GYRO: readL3G4200D(); break;
case SEN_COMPASS: readHMC5883L(); break;
}
return 0;
}
void setActuator(char type, int value){
switch (type){
case ACT_MOTOR_MOW: analogWrite(pinMotorMow, 255-value); break;
case ACT_MOTOR_LEFT: setL298(pinMotorLeftDir, pinMotorLeftPWM, value);
case ACT_MOTOR_RIGHT: setL298(pinMotorRightDir, pinMotorRightPWM, value); break;
}
}