Skip to content

Fix typo and indentetion #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
PaoloP74 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Fix typo and indentetion #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 23 additions & 23 deletions Arduino/BROBOT_EVO2/BROBOT_EVO2.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
// OSC controls:
// fader1: Throttle (0.0-1.0) OSC message: /1/fader1
// fader2: Steering (0.0-1.0) OSC message: /1/fader2
// push1: Move servo arm (and robot raiseup) OSC message /1/push1
// push1: Move servo arm (and robot raiseup) OSC message /1/push1
// if you enable the touchMessage on TouchOSC options, controls return to center automatically when you lift your fingers
// PRO mode (PRO button). On PRO mode steering and throttle are more aggressive
// PAGE2: PID adjustements [optional][dont touch if you dont know what you are doing...;-) ]
Expand All @@ -51,7 +51,7 @@
//#define WIFI_SSID "YOUR_WIFI"
//#define WIFI_PASSWORD "YOUR_PASSWORD"
//#define WIFI_IP "192.168.1.101" // Force ROBOT IP
//#define TELEMETRY "192.168.1.38" // Tlemetry server port 2223
//#define TELEMETRY "192.168.1.38" // Telemetry server port 2223

#define TELEMETRY "192.168.4.2" // Default telemetry server (first client) port 2223

Expand All @@ -66,17 +66,17 @@
#define MAX_TARGET_ANGLE_PRO 26 // Max recommended value: 32

// Default control terms for EVO 2
#define KP 0.32
#define KD 0.050
#define KP_THROTTLE 0.080
#define KI_THROTTLE 0.1
#define KP_POSITION 0.06
#define KD_POSITION 0.45
#define KP 0.32
#define KD 0.050
#define KP_THROTTLE 0.080
#define KI_THROTTLE 0.1
#define KP_POSITION 0.06
#define KD_POSITION 0.45
//#define KI_POSITION 0.02

// Control gains for raiseup (the raiseup movement requiere special control parameters)
#define KP_RAISEUP 0.1
#define KD_RAISEUP 0.16
#define KP_RAISEUP 0.1
#define KD_RAISEUP 0.16
#define KP_THROTTLE_RAISEUP 0 // No speed control on raiseup
#define KI_THROTTLE_RAISEUP 0.0

Expand Down Expand Up @@ -128,7 +128,7 @@ float dt;
// Angle of the robot (used for stability control)
float angle_adjusted;
float angle_adjusted_Old;
float angle_adjusted_filtered=0.0;
float angle_adjusted_filtered = 0.0;

// Default control values from constant definitions
float Kp = KP;
Expand Down Expand Up @@ -252,7 +252,7 @@ void setup()
ESPsendCommand(auxCommand, "OK", 4);
#endif
ESPsendCommand("AT+CIPSTA?", "OK", 4);
#else // Deafault : we generate a wifi network
#else // Default : we generate a wifi network
Serial1.println("AT+CIPSTAMAC?");
ESPgetMac();
//Serial.print("MAC:");
Expand All @@ -272,10 +272,10 @@ void setup()
ESPsendCommand("AT+CIPMUX=0", "OK", 3); // Single connection mode
ESPsendCommand("AT+CIPMODE=1", "OK", 3); // Transparent mode
char Telemetry[80];
strcpy(Telemetry,"AT+CIPSTART=\"UDP\",\"");
strcat(Telemetry,TELEMETRY);
strcat(Telemetry,"\",2223,2222,0");
ESPsendCommand(Telemetry, "OK", 3);
strcpy(Telemetry, "AT+CIPSTART=\"UDP\",\"");
strcat(Telemetry, TELEMETRY);
strcat(Telemetry, "\",2223,2222,0");
ESPsendCommand(Telemetry, "OK", 3);

// Calibrate gyros
MPU6050_calibrate();
Expand Down Expand Up @@ -308,7 +308,7 @@ void setup()
digitalWrite(4, LOW); // Enable stepper drivers
// Enable TIMERs interrupts
TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt
TIMSK3 |= (1 << OCIE1A); // Enable Timer1 interrupt
TIMSK3 |= (1 << OCIE1A); // Enable Timer3 interrupt

// Little motor vibration and servo move to indicate that robot is ready
for (uint8_t k = 0; k < 5; k++)
Expand All @@ -327,7 +327,7 @@ void setup()
BROBOT_moveServo1(SERVO_AUX_NEUTRO);
BROBOT_moveServo2(SERVO2_NEUTRO);

#if TELEMETRY_BATTERY==1
#if TELEMETRY_BATTERY==1
BatteryValue = BROBOT_readBattery(true);
Serial.print("BATT:");
Serial.println(BatteryValue);
Expand Down Expand Up @@ -424,9 +424,9 @@ void loop()
// Get new orientation angle from IMU (MPU6050)
float MPU_sensor_angle = MPU6050_getAngle(dt);
angle_adjusted = MPU_sensor_angle + angle_offset;
if ((MPU_sensor_angle>-15)&&(MPU_sensor_angle<15))
angle_adjusted_filtered = angle_adjusted_filtered*0.99 + MPU_sensor_angle*0.01;
if ((MPU_sensor_angle > -15) && (MPU_sensor_angle < 15))
angle_adjusted_filtered = angle_adjusted_filtered * 0.99 + MPU_sensor_angle * 0.01;

#if DEBUG==1
Serial.print(dt);
Serial.print(" ");
Expand All @@ -440,7 +440,7 @@ void loop()

// We calculate the estimated robot speed:
// Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward

int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 25.0; // 25 is an empirical extracted factor to adjust for real units
int16_t estimated_speed = -actual_robot_speed + angular_velocity;
Expand Down Expand Up @@ -564,7 +564,7 @@ void loop()
// Telemetry here?
#if TELEMETRY_ANGLE==1
char auxS[25];
int ang_out = constrain(int(angle_adjusted * 10),-900,900);
int ang_out = constrain(int(angle_adjusted * 10), -900, 900);
sprintf(auxS, "$tA,%+04d", ang_out);
Serial1.println(auxS);
#endif
Expand Down
4 changes: 2 additions & 2 deletions Arduino/BROBOT_EVO2/MPU6050.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -500,9 +500,9 @@ void MPU6050_calibrate()
float dev;
int16_t values[100];
bool gyro_cal_ok = false;

delay(500);
while (!gyro_cal_ok){
while (!gyro_cal_ok) {
Serial.println("Gyro calibration... DONT MOVE!");
// we take 100 measurements in 4 seconds
for (i = 0; i < 100; i++)
Expand Down
4 changes: 1 addition & 3 deletions Arduino/BROBOT_EVO2/Network.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ void readControlParameters()
#endif

// Calibration mode??
if (OSCpush[2]==1)
if (OSCpush[2] == 1)
{
Serial.print("Calibration MODE ");
angle_offset = angle_adjusted_filtered;
Expand Down Expand Up @@ -148,5 +148,3 @@ int ESPsendCommand(char *command, String stopstr, int timeout_secs)
delay(250);
}



29 changes: 14 additions & 15 deletions Arduino/BROBOT_EVO2/Servos.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,16 +23,16 @@ int battery;
// Resolution: 8us per step (this is OK for servos, around 175 steps for typical servo)
void BROBOT_initServo()
{
int temp;
int temp;

// Initialize Timer4 as Fast PWM
TCCR4A = (1<<PWM4A)|(1<<PWM4B);
TCCR4A = (1 << PWM4A) | (1 << PWM4B);
TCCR4B = 0;
TCCR4C = (1<<PWM4D);
TCCR4C = (1 << PWM4D);
TCCR4D = 0;
TCCR4E = (1<<ENHC4); // Enhaced -> 11 bits
TCCR4E = (1 << ENHC4); // Enhaced -> 11 bits

temp = 1500>>3;
temp = 1500 >> 3;
TC4H = temp >> 8;
OCR4B = temp & 0xff;

Expand All @@ -51,25 +51,25 @@ int temp;
DDRD |= (1 << 7); // OC4D = PD7 (Pin6 on Leonardo board)

//Enable OC4A and OC4B and OCR4D output
TCCR4A |= (1<<COM4B1)|(1<<COM4A1);
TCCR4C |= (1<<COM4D1);
TCCR4A |= (1 << COM4B1) | (1 << COM4A1);
TCCR4C |= (1 << COM4D1);
// set prescaler to 256 and enable timer 16Mhz/256/1024 = 61Hz (16.3ms)
TCCR4B = (1 << CS43)|(1 << CS40);
TCCR4B = (1 << CS43) | (1 << CS40);
}

void BROBOT_moveServo1(int pwm)
{
pwm = constrain(pwm,SERVO1_MIN_PULSEWIDTH,SERVO1_MAX_PULSEWIDTH)>>3; // Check max values and Resolution: 8us
pwm = constrain(pwm, SERVO1_MIN_PULSEWIDTH, SERVO1_MAX_PULSEWIDTH) >> 3; // Check max values and Resolution: 8us
// 11 bits => 3 MSB bits on TC4H, LSB bits on OCR4B
TC4H = pwm>>8;
TC4H = pwm >> 8;
OCR4B = pwm & 0xFF;
}

void BROBOT_moveServo2(int pwm)
{
pwm = constrain(pwm,SERVO2_MIN_PULSEWIDTH,SERVO2_MAX_PULSEWIDTH)>>3; // Check max values and Resolution: 8us
pwm = constrain(pwm, SERVO2_MIN_PULSEWIDTH, SERVO2_MAX_PULSEWIDTH) >> 3; // Check max values and Resolution: 8us
// 11 bits => 3 MSB bits on TC4H, LSB bits on OCR4B
TC4H = pwm>>8;
TC4H = pwm >> 8;
OCR4A = pwm & 0xFF; // 2.0 or 2.3 boards servo2 output
OCR4D = pwm & 0xFF; // 2.1 or 2.4 boards servo2 output
}
Expand All @@ -78,10 +78,9 @@ void BROBOT_moveServo2(int pwm)
int BROBOT_readBattery(bool first_time)
{
if (first_time)
battery = analogRead(5)/BATT_VOLT_FACTOR;
battery = analogRead(5) / BATT_VOLT_FACTOR;
else
battery = (battery*9 + (analogRead(5)/BATT_VOLT_FACTOR))/10;
battery = (battery * 9 + (analogRead(5) / BATT_VOLT_FACTOR)) / 10;
return battery;
}