diff --git a/FightTactics.ino b/FightTactics.ino index abd26d4..2e9c86f 100644 --- a/FightTactics.ino +++ b/FightTactics.ino @@ -1,6 +1,7 @@ -/** - * @function +/* + * @function searchPID : PID control loop implementation. */ + bool searchPID(){ measure = ((enemy_sensor_left + enemy_sensor_f_left + enemy_sensor_front + enemy_sensor_f_right + enemy_sensor_right) == 0)?(measure):(enemy_sensor_left*1000 + enemy_sensor_f_left*2000 + enemy_sensor_front*3000 + enemy_sensor_f_right*4000 + enemy_sensor_right*5000)/(enemy_sensor_left + enemy_sensor_f_left + enemy_sensor_front + enemy_sensor_f_right + enemy_sensor_right); error = ref - measure; @@ -63,8 +64,12 @@ void fight1(){ if(actuation < POWER_MAX){ while(millis() < next){ actuation = (actuation >= POWER_MAX)?(POWER_MAX):(actuation+10); - ST.motor(1, actuation); - ST.motor(2, actuation); + + #if ENABLE > NONE + ST.motor(1, actuation); + ST.motor(2, actuation); + #endif + delay(20); /* if(measureLine()){ @@ -75,20 +80,32 @@ void fight1(){ } } else{ - ST.motor(1, POWER_MAX); - ST.motor(2, POWER_MAX); + + #if ENABLE > NONE + ST.motor(1, POWER_MAX); + ST.motor(2, POWER_MAX); + #endif + } return; } void fight2(){ - ST.motor(1, POWER_MAX/1); - ST.motor(2, POWER_MAX/1); + + #if ENABLE > NONE + ST.motor(1, POWER_MAX/1); + ST.motor(2, POWER_MAX/1); + #endif + return; } void searchFWD(){ - ST.motor(1, POWER_MAX*0.75); - ST.motor(2, POWER_MAX*0.75); + + #if ENABLE > NONE + ST.motor(1, POWER_MAX*0.75); + ST.motor(2, POWER_MAX*0.75); + #endif + delay(20); } diff --git a/MeasureTactics.ino b/MeasureTactics.ino index e177890..55ec5da 100644 --- a/MeasureTactics.ino +++ b/MeasureTactics.ino @@ -1,7 +1,15 @@ -/** - * @function measureEnemy : This function reads the diffuse reflex sensors and determines an enemy is detected. - * @param : none - * @return : Returns 0 if an enemy is detected, 1 otherwise. +/* + * @function measureEnemy : This function reads the diffuse reflex sensors and determines if an enemy is detected. + * @param : none. + * @return : Returns 1 if an enemy is detected, 0 otherwise. + * + * @function rc_read_values : This function reads the given RC channels. + * @param : none. + * @return : none. + * + * @function calc_input : This function calculates the the input of a pin given an RC channel. + * @param : RC channel and RC input pin + * @return : none. */ @@ -46,8 +54,7 @@ void calc_input(uint8_t channel, uint8_t input_pin) { } } - -void calc_ch1() { calc_input(DIRECTION, DIRECTION_INPUT); } +void calc_ch1() { calc_input(STEERING, STEERING_INPUT); } void calc_ch2() { calc_input(THROTTLE, THROTTLE_INPUT); } void calc_ch3() { calc_input(SWITCH, SWITCH_INPUT); } void calc_ch4() { calc_input(BUTTON, BUTTON_INPUT); } diff --git a/docs/Sabertooth2x25.pdf b/docs/Sabertooth2x25.pdf new file mode 100644 index 0000000..1e7d60d Binary files /dev/null and b/docs/Sabertooth2x25.pdf differ diff --git a/sumo-rc.ino b/sumo-rc.ino index 6dda2f5..b35c1bb 100644 --- a/sumo-rc.ino +++ b/sumo-rc.ino @@ -1,24 +1,25 @@ -/** - * @brief: Radio-Controlled Sumo robot test code. +/* + * @brief: Radio-Controlled Mega Sumo Robot code. * @author: Lucas Cortés Gutiérrez. - * @date: 2019/11/10 + * @date: 2020/04/16 */ #include #include -#define NONE 0 -#define ENABLE 1 // Change this to 1 when testing for real. -#define DEBUG !ENABLE // When enabled, this will print via serial prompt all the measurements and messages written into this file, as well as the "Tactic's" files. +#define ENABLE 1 // Change this to 1 when testing for real. +#define DEBUG !ENABLE // When enabled this will print via serial prompt all the measurements and messages written into this file, as well as the "Tactic's" files. +#define NONE 0 // Definition of NONE as zero. #define DRIFT 50 // Stick drift dead-zone. #define THRESHOLD 50 // Switch threshold. #define TURN_TIME 300 // Time (in mili-seconds) needed for a 180 degree turn. #define DIVIDER 1 // POWER_MAX divider. -#define POWER_MAX 255 // Maximum power supplied through the motor driver. +#define POWER_MAX 127 // Maximum power supplied through the motor driver. double POWER = 0; +double DIRECTION = 0; -/** +/* * @brief : Reflex sensors pinout definition */ @@ -32,8 +33,8 @@ double POWER = 0; #define MISS_COUNT_MAX 1000 /** - * @brief : PID controller constants. - * We used the Ziegler–Nichols method to tune this parameters. + * @brief : PID controller factors. + * The Ziegler–Nichols method is used to tune this parameters. */ #define KP 0.032 @@ -66,16 +67,21 @@ int tactic = 0; #define RC_NUM_CHANNELS 4 -#define DIRECTION 0 +#define STEERING 0 #define THROTTLE 1 #define SWITCH 2 #define BUTTON 3 -#define DIRECTION_INPUT A0 // Right stick, X axis +#define STEERING_INPUT A0 // Right stick, X axis #define THROTTLE_INPUT A1 // Left stick, Y axis #define SWITCH_INPUT A2 // SWA & SWD #define BUTTON_INPUT A3 // KEY2 +double STEERING_VALUE = 0; +double THROTTLE_VALUE = 0; +double SWITCH_VALUE = 0; +double BUTTON_VALUE = 0; + uint16_t rc_values[RC_NUM_CHANNELS]; uint32_t rc_start[RC_NUM_CHANNELS]; volatile uint16_t rc_shared[RC_NUM_CHANNELS]; @@ -83,12 +89,12 @@ volatile uint16_t rc_shared[RC_NUM_CHANNELS]; Sabertooth ST(128); void setup(){ - pinMode(DIRECTION_INPUT, INPUT); + pinMode(STEERING_INPUT, INPUT); pinMode(THROTTLE_INPUT, INPUT); pinMode(SWITCH_INPUT, INPUT); pinMode(BUTTON_INPUT, INPUT); - enableInterrupt(DIRECTION_INPUT, calc_ch1, CHANGE); + enableInterrupt(STEERING_INPUT, calc_ch1, CHANGE); enableInterrupt(THROTTLE_INPUT, calc_ch2, CHANGE); enableInterrupt(SWITCH_INPUT, calc_ch3, CHANGE); enableInterrupt(BUTTON_INPUT, calc_ch4, CHANGE); @@ -115,7 +121,7 @@ void setup(){ #endif while ((rc_values[SWITCH] < 1500 - THRESHOLD) || (rc_values[SWITCH] > 1500 + THRESHOLD)) rc_read_values(); - digitalWrite(LED_BUILTIN, HIGH); + digitalWrite(LED_BUILTIN, HIGH); // This can be changed to an actual led to signal that the robot is ready to fight. #if DEBUG > NONE Serial.println("START!"); @@ -125,19 +131,27 @@ void setup(){ void loop(){ rc_read_values(); + STEERING_VALUE = rc_values[STEERING]; + THROTTLE_VALUE = rc_values[THROTTLE]; + SWITCH_VALUE = rc_values[SWITCH]; + BUTTON_VALUE = rc_values[BUTTON]; + + POWER = ((THROTTLE_VALUE < 1500 - DRIFT) || (THROTTLE_VALUE > 1500 + DRIFT)) ? (THROTTLE_VALUE - 1500.0)/(4*DIVIDER) : 0; + DIRECTION = ((STEERING_VALUE < 1500 - DRIFT) || (STEERING_VALUE > 1500 + DRIFT)) ? (STEERING_VALUE - 1500.0)/(4*DIVIDER) : 0; + #if DEBUG > NONE - Serial.print("DIRECTION:"); Serial.print(rc_values[DIRECTION]); Serial.print("\t"); - Serial.print("THROTTLE:"); Serial.print(rc_values[THROTTLE]); Serial.print("\t"); - Serial.print("SWITCHES:"); Serial.print(rc_values[SWITCH]); Serial.print("\t"); - Serial.print("BUTTON:"); Serial.println(rc_values[BUTTON]); + Serial.print("STEERING:"); Serial.print(STEERING_VALUE); Serial.print("\t"); + Serial.print("THROTTLE:"); Serial.print(THROTTLE_VALUE); Serial.print("\t"); + Serial.print("SWITCHES:"); Serial.print(SWITCH_VALUE); Serial.print("\t"); + Serial.print("BUTTON:"); Serial.println(BUTTON_VALUE); Serial.print("\t"); + Serial.print("BUTTON:"); Serial.println(POWER); Serial.print("\t"); + Serial.print("BUTTON:"); Serial.println(DIRECTION); #endif - if ((rc_values[SWITCH] > 1500 - THRESHOLD) && (rc_values[SWITCH] < 1500 + THRESHOLD)) { + if ((SWITCH_VALUE > 1500 - THRESHOLD) && (SWITCH_VALUE < 1500 + THRESHOLD)) { - POWER = ((rc_values[THROTTLE] < 1500 - DRIFT) || (rc_values[THROTTLE] > 1500 + DRIFT)) ? (rc_values[THROTTLE] - 1500.0)/(2*DIVIDER) : 0; + if ((BUTTON_VALUE > 2000 - THRESHOLD) && (BUTTON_VALUE < 2000 + THRESHOLD)) { - - if ((rc_values[BUTTON] > 2000 - THRESHOLD) && (rc_values[BUTTON] < 2000 + THRESHOLD)) { #if ENABLE > NONE ST.motor(1, POWER_MAX/DIVIDER); ST.motor(2, -POWER_MAX/DIVIDER); @@ -149,7 +163,8 @@ void loop(){ #endif } - if ((rc_values[DIRECTION] > 1500 - DRIFT) && (rc_values[DIRECTION] < 1500 + DRIFT)) { + if (DIRECTION == 0) { + #if ENABLE > NONE ST.motor(1, POWER); ST.motor(2, POWER); @@ -162,24 +177,27 @@ void loop(){ #endif } - else if (rc_values[DIRECTION] > 1500 + DRIFT) { + else if (DIRECTION > 0) { + #if ENABLE > NONE - ST.motor(1, POWER); - ST.motor(2, -POWER); + ST.motor(1, POWER+DIRECTION); + ST.motor(2, POWER-DIRECTION); #endif #if DEBUG > NONE - Serial.print("RIGHT : "); Serial.println(POWER); + Serial.print("RIGHT : "); Serial.println(POWER+DIRECTION); #endif } - else if (rc_values[DIRECTION] < 1500 - DRIFT) { + else if (DIRECTION < 0) { + #if ENABLE > NONE - ST.motor(1, -POWER); - ST.motor(2, POWER); + ST.motor(1, POWER-DIRECTION); + ST.motor(2, POWER+DIRECTION); #endif + #if DEBUG > NONE - Serial.print("LEFT : "); Serial.println(POWER); + Serial.print("LEFT : "); Serial.println(POWER+DIRECTION); #endif } @@ -188,14 +206,14 @@ void loop(){ #endif } - else if ((rc_values[SWITCH] > 2000 - THRESHOLD) && (rc_values[SWITCH] < 2000 + THRESHOLD)){ + else if ((SWITCH_VALUE > 2000 - THRESHOLD) && (SWITCH_VALUE < 2000 + THRESHOLD)){ if(measureEnemy()){ if(searchPID()){ - POWER = (rc_values[THROTTLE] > 1500 + DRIFT) ? (rc_values[THROTTLE] - 1500.0)/(2*DIVIDER) : 0; // Be aware that the power definition is different from the one in line 134, this is due to our need of using only positive values, nonetheless, this can be changed if the need arise. - + POWER = (POWER > 0) ? POWER : 0; + //fight(); #if ENABLE > NONE ST.motor(1, POWER); ST.motor(2, POWER); @@ -205,7 +223,7 @@ void loop(){ if (POWER == 0) {Serial.println("IDLE");} else if (POWER > 0) {Serial.print("FIGHTING : "); Serial.println(POWER);} #endif - //fight(); + } } }