Skip to content

Commit

Permalink
Main file updated
Browse files Browse the repository at this point in the history
  • Loading branch information
@lcortesg
lcortesg committed Apr 16, 2020
1 parent c08a5dd commit b4ab606
Show file tree
Hide file tree
Showing 4 changed files with 94 additions and 52 deletions.
37 changes: 27 additions & 10 deletions FightTactics.ino
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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()){
Expand All @@ -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);
}
19 changes: 13 additions & 6 deletions MeasureTactics.ino
View file
Original file line number Diff line number Diff line change
@@ -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.
*/


Expand Down Expand Up @@ -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); }
Binary file added docs/Sabertooth2x25.pdf
View file
Binary file not shown.
90 changes: 54 additions & 36 deletions sumo-rc.ino
View file
Original file line number Diff line number Diff line change
@@ -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 <EnableInterrupt.h>
#include <Sabertooth.h>

#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
*/

Expand All @@ -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
Expand Down Expand Up @@ -66,29 +67,34 @@ 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];

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);
Expand All @@ -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!");
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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
}

Expand All @@ -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);
Expand All @@ -205,7 +223,7 @@ void loop(){
if (POWER == 0) {Serial.println("IDLE");}
else if (POWER > 0) {Serial.print("FIGHTING : "); Serial.println(POWER);}
#endif
//fight();

}
}
}
Expand Down

0 comments on commit b4ab606

Please sign in to comment.