Mirrors/B-ROBOT_EVO2
2
0
Fork 0
mirror of https://github.com/jjrobots/B-ROBOT_EVO2.git synced 2026-02-25 22:01:22 +01:00
Code Issues Packages Projects Releases Wiki Activity

B-ROBOT EVO2

Browse Source 
First upload: code, electronics...
This commit is contained in:
JJROBOTS
2017-03-29 17:38:40 +01:00
parent e6771a3945
commit 0ff1d58c97
7 changed files with 8416 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

580
Arduino/BROBOT_EVO2/BROBOT_EVO2.ino Normal file
View File

@@ -0,0 +1,580 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// JJROBOTS BROBOT KIT: (Arduino Leonardo + BROBOT ELECTRONIC BRAIN SHIELD + STEPPER MOTOR drivers)
// This code is prepared for new BROBOT shield with ESP8266 Wifi module
// This code doesn´t need external libraries
// Author: JJROBOTS.COM
// Date: 02/09/2014
// Updated: 08/03/2017
// Version: 2.8
// License: GPL v2
// Project URL: http://jjrobots.com/b-robot (Features,documentation,build instructions,how it works, SHOP,...)
// New updates:
// - New default parameters specially tuned for BROBOT EVO 2 version
// - New Move mode with position control (for externally programming the robot)
// - New telemtry packets send to TELEMETRY IP for monitoring Battery, Angle, ... (old battery packets for touch osc not working now)
// - Default telemetry server is 192.168.4.2 (first client connected to the robot)
// Get the free android app (jjrobots) from google play. For IOS users you need to use TouchOSC App + special template (info on jjrobots page)
// Thanks to our users on the forum for the new ideas. Specially sasa999, KomX, ...
// The board needs at least 10-15 seconds with no motion (robot steady) at beginning to give good values...
// MPU6050 IMU connected via I2C bus. Angle estimation using complementary filter (fusion between gyro and accel)
// Angle calculations and control part is running at 100Hz
// The robot is OFF when the angle is high (robot is horizontal). When you start raising the robot it
// automatically switch ON and start a RAISE UP procedure.
// You could RAISE UP the robot also with the robot arm servo (Servo button on the interface)
// To switch OFF the robot you could manually put the robot down on the floor (horizontal)
// We use a standard PID controllers (Proportional, Integral derivative controller) for robot stability
// More info on the project page: How it works page at jjrobots.com
// We have a PI controller for speed control and a PD controller for stability (robot angle)
// The output of the control (motors speed) is integrated so it´s really an acceleration not an speed.
// We control the robot from a WIFI module using OSC standard UDP messages
// You need an OSC app to control de robot (Custom free JJRobots APP for android, and TouchOSC APP for IOS)
// Join the module Wifi Access Point (by default: JJROBOTS_XX) with your Smartphone/Tablet...
// Wifi password: 87654321
// For TouchOSC users (IOS): Install the BROBOT layout into the OSC app (Touch OSC) and start play! (read the project page)
// 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
// 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...;-) ]
#include <Wire.h>
// Uncomment this lines to connect to an external Wifi router (join an existing Wifi network)
//#define EXTERNAL_WIFI
//#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.4.2" // Default telemetry server (first client) port 2223
// NORMAL MODE PARAMETERS (MAXIMUN SETTINGS)
#define MAX_THROTTLE 550
#define MAX_STEERING 140
#define MAX_TARGET_ANGLE 14
// PRO MODE = MORE AGGRESSIVE (MAXIMUN SETTINGS)
#define MAX_THROTTLE_PRO 860
#define MAX_STEERING_PRO 280
#define MAX_TARGET_ANGLE_PRO 32
// Default control terms for EVO 2
#define KP 0.32
#define KD 0.050
#define KP_THROTTLE 0.075
#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_THROTTLE_RAISEUP 0 // No speed control on raiseup
#define KI_THROTTLE_RAISEUP 0.0
#define MAX_CONTROL_OUTPUT 500
#define ITERM_MAX_ERROR 30 // Iterm windup constants for PI control
#define ITERM_MAX 10000
#define ANGLE_OFFSET -1.0 // Offset angle for balance (to compensate robot own weitght distribution)
// Servo definitions
#define SERVO_AUX_NEUTRO 1500 // Servo neutral position
#define SERVO_MIN_PULSEWIDTH 700
#define SERVO_MAX_PULSEWIDTH 2500
// Telemetry
#define TELEMETRY_BATTERY 1
#define TELEMETRY_ANGLE 1
//#define TELEMETRY_DEBUG 1 // Dont use TELEMETRY_ANGLE and TELEMETRY_DEBUG at the same time!
#define ZERO_SPEED 65535
#define MAX_ACCEL 15 // Maximun motor acceleration (MAX RECOMMENDED VALUE: 20) (default:15)
#define MICROSTEPPING 16 // 8 or 16 for 1/8 or 1/16 driver microstepping (default:16)
#define DEBUG 0 // 0 = No debug info (default) DEBUG 1 for console output
// AUX definitions
#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))
#define RAD2GRAD 57.2957795
#define GRAD2RAD 0.01745329251994329576923690768489
String MAC; // MAC address of Wifi module
uint8_t cascade_control_loop_counter = 0;
uint8_t loop_counter; // To generate a medium loop 40Hz
uint8_t slow_loop_counter; // slow loop 2Hz
uint8_t sendBattery_counter; // To send battery status
int16_t BatteryValue;
long timer_old;
long timer_value;
float debugVariable;
float dt;
// Angle of the robot (used for stability control)
float angle_adjusted;
float angle_adjusted_Old;
// Default control values from constant definitions
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE;
float Ki_thr = KI_THROTTLE;
float Kp_user = KP;
float Kd_user = KD;
float Kp_thr_user = KP_THROTTLE;
float Ki_thr_user = KI_THROTTLE;
float Kp_position = KP_POSITION;
float Kd_position = KD_POSITION;
bool newControlParameters = false;
bool modifing_control_parameters = false;
int16_t position_error_sum_M1;
int16_t position_error_sum_M2;
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
int16_t throttle;
float steering;
float max_throttle = MAX_THROTTLE;
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;
boolean positionControlMode = false;
uint8_t mode; // mode = 0 Normal mode, mode = 1 Pro mode (More agressive)
int16_t motor1;
int16_t motor2;
// position control
volatile int32_t steps1;
volatile int32_t steps2;
int32_t target_steps1;
int32_t target_steps2;
int16_t motor1_control;
int16_t motor2_control;
int16_t speed_M1, speed_M2; // Actual speed of motors
int8_t dir_M1, dir_M2; // Actual direction of steppers motors
int16_t actual_robot_speed; // overall robot speed (measured from steppers speed)
int16_t actual_robot_speed_Old;
float estimated_speed_filtered; // Estimated robot speed
// OSC output variables
uint8_t OSCpage;
uint8_t OSCnewMessage;
float OSCfader[4];
float OSCxy1_x;
float OSCxy1_y;
float OSCxy2_x;
float OSCxy2_y;
uint8_t OSCpush[4];
uint8_t OSCtoggle[4];
uint8_t OSCmove_mode;
int16_t OSCmove_speed;
int16_t OSCmove_steps1;
int16_t OSCmove_steps2;
// INITIALIZATION
void setup()
{
// STEPPER PINS ON JJROBOTS BROBOT BRAIN BOARD
pinMode(4, OUTPUT); // ENABLE MOTORS
pinMode(7, OUTPUT); // STEP MOTOR 1 PORTE,6
pinMode(8, OUTPUT); // DIR MOTOR 1 PORTB,4
pinMode(12, OUTPUT); // STEP MOTOR 2 PORTD,6
pinMode(5, OUTPUT); // DIR MOTOR 2 PORTC,6
digitalWrite(4, HIGH); // Disbale motors
pinMode(10, OUTPUT); // Servo1 (arm)
pinMode(13, OUTPUT); // Servo2
Serial.begin(115200); // Serial output to console
Serial1.begin(115200);
OSC_init();
// Initialize I2C bus (MPU6050 is connected via I2C)
Wire.begin();
#if DEBUG > 0
delay(9000);
#else
delay(1000);
#endif
Serial.println("BROBOT by JJROBOTS v2.8");
delay(200);
Serial.println("Don't move for 10 sec...");
MPU6050_setup(); // setup MPU6050 IMU
delay(500);
// With the new ESP8266 WIFI MODULE WE NEED TO MAKE AN INITIALIZATION PROCESS
Serial.println("WIFI init");
Serial1.flush();
Serial1.print("+++"); // To ensure we exit the transparent transmision mode
delay(100);
ESPsendCommand("AT", "OK", 1);
ESPsendCommand("AT+RST", "OK", 2); // ESP Wifi module RESET
ESPwait("ready", 6);
ESPsendCommand("AT+GMR", "OK", 5);
#ifdef EXTERNAL_WIFI
ESPsendCommand("AT+CWQAP", "OK", 3);
ESPsendCommand("AT+CWMODE=1", "OK", 3);
//String auxCommand = (String)"AT+CWJAP="+WIFI_SSID+","+WIFI_PASSWORD;
char auxCommand[90] = "AT+CWJAP=\"";
strcat(auxCommand, WIFI_SSID);
strcat(auxCommand, "\",\"");
strcat(auxCommand, WIFI_PASSWORD);
strcat(auxCommand, "\"");
ESPsendCommand(auxCommand, "OK", 14);
#ifdef WIFI_IP
strcpy(auxCommand, "AT+CIPSTA=\"");
strcat(auxCommand, WIFI_IP);
strcat(auxCommand, "\"");
ESPsendCommand(auxCommand, "OK", 4);
#endif
ESPsendCommand("AT+CIPSTA?", "OK", 4);
#else // Deafault : we generate a wifi network
Serial1.println("AT+CIPSTAMAC?");
ESPgetMac();
//Serial.print("MAC:");
//Serial.println(MAC);
delay(200);
ESPsendCommand("AT+CWQAP", "OK", 3);
ESPsendCommand("AT+CWMODE=2", "OK", 3); // Soft AP mode
// Generate Soft AP. SSID=JJROBOTS, PASS=87654321
char *cmd = "AT+CWSAP=\"JJROBOTS_XX\",\"87654321\",5,3";
// Update XX characters with MAC address (last 2 characters)
cmd[19] = MAC[10];
cmd[20] = MAC[11];
ESPsendCommand(cmd, "OK", 6);
#endif
// Start UDP SERVER on port 2222, telemetry port 2223
//Serial.println("Start UDP server");
//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);
// Start TCP SERVER
ESPsendCommand("AT+CIPMUX=1", "OK", 3); // Single connection mode
ESPsendCommand("AT+CIPMODE=1", "OK", 3); // Transparent mode
ESPsendCommand("AT+CIPSERVER=1,2222", "OK", 3); // TCP server
// Calibrate gyros
MPU6050_calibrate();
ESPsendCommand("AT+CIPSEND", ">", 2); // Start transmission (transparent mode)
// Init servos
Serial.println("Servo init");
BROBOT_initServo();
BROBOT_moveServo1(SERVO_AUX_NEUTRO);
// STEPPER MOTORS INITIALIZATION
Serial.println("Stepers init");
// MOTOR1 => TIMER1
TCCR1A = 0; // Timer1 CTC mode 4, OCxA,B outputs disconnected
TCCR1B = (1 << WGM12) | (1 << CS11); // Prescaler=8, => 2Mhz
OCR1A = ZERO_SPEED; // Motor stopped
dir_M1 = 0;
TCNT1 = 0;
// MOTOR2 => TIMER3
TCCR3A = 0; // Timer3 CTC mode 4, OCxA,B outputs disconnected
TCCR3B = (1 << WGM32) | (1 << CS31); // Prescaler=8, => 2Mhz
OCR3A = ZERO_SPEED; // Motor stopped
dir_M2 = 0;
TCNT3 = 0;
delay(200);
// Enable stepper drivers and TIMER interrupts
digitalWrite(4, LOW); // Enable stepper drivers
// Enable TIMERs interrupts
TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt
TIMSK3 |= (1 << OCIE1A); // Enable Timer1 interrupt
// Little motor vibration and servo move to indicate that robot is ready
for (uint8_t k = 0; k < 5; k++)
{
setMotorSpeedM1(5);
setMotorSpeedM2(5);
BROBOT_moveServo1(SERVO_AUX_NEUTRO + 100);
delay(200);
setMotorSpeedM1(-5);
setMotorSpeedM2(-5);
BROBOT_moveServo1(SERVO_AUX_NEUTRO - 100);
delay(200);
}
BROBOT_moveServo1(SERVO_AUX_NEUTRO);
#if TELEMETRY_BATTERY==1
BatteryValue = BROBOT_readBattery(true);
Serial.print("BATT:");
Serial.println(BatteryValue);
#endif
Serial.println("Start...");
timer_old = micros();
}
// MAIN LOOP
void loop()
{
OSC_MsgRead(); // Read UDP OSC messages
if (OSCnewMessage)
{
OSCnewMessage = 0;
if (OSCpage == 1) // Get commands from user (PAGE1 are user commands: throttle, steering...)
{
if (modifing_control_parameters) // We came from the settings screen
{
OSCfader[0] = 0.5; // default neutral values
OSCfader[1] = 0.5;
OSCtoggle[0] = 0; // Normal mode
mode = 0;
modifing_control_parameters = false;
}
if (OSCmove_mode)
{
//Serial.print("M ");
//Serial.print(OSCmove_speed);
//Serial.print(" ");
//Serial.print(OSCmove_steps1);
//Serial.print(",");
//Serial.println(OSCmove_steps2);
positionControlMode = true;
OSCmove_mode = false;
target_steps1 = steps1 + OSCmove_steps1;
target_steps2 = steps2 + OSCmove_steps2;
}
else
{
positionControlMode = false;
throttle = (OSCfader[0] - 0.5) * max_throttle;
// We add some exponential on steering to smooth the center band
steering = OSCfader[1] - 0.5;
if (steering > 0)
steering = (steering * steering + 0.5 * steering) * max_steering;
else
steering = (-steering * steering + 0.5 * steering) * max_steering;
}
if ((mode == 0) && (OSCtoggle[0]))
{
// Change to PRO mode
max_throttle = MAX_THROTTLE_PRO;
max_steering = MAX_STEERING_PRO;
max_target_angle = MAX_TARGET_ANGLE_PRO;
mode = 1;
}
if ((mode == 1) && (OSCtoggle[0] == 0))
{
// Change to NORMAL mode
max_throttle = MAX_THROTTLE;
max_steering = MAX_STEERING;
max_target_angle = MAX_TARGET_ANGLE;
mode = 0;
}
}
else if (OSCpage == 2) { // OSC page 2
// Check for new user control parameters
readControlParameters();
}
#if DEBUG==1
Serial.print(throttle);
Serial.print(" ");
Serial.println(steering);
#endif
} // End new OSC message
timer_value = micros();
// New IMU data?
if (MPU6050_newData())
{
MPU6050_read_3axis();
loop_counter++;
slow_loop_counter++;
dt = (timer_value - timer_old) * 0.000001; // dt in seconds
timer_old = timer_value;
angle_adjusted_Old = angle_adjusted;
// Get new orientation angle from IMU (MPU6050)
angle_adjusted = MPU6050_getAngle(dt) + ANGLE_OFFSET;
#if DEBUG==1
Serial.print(dt);
Serial.print(" ");
Serial.println(angle_adjusted);
#endif
//Serial.print("\t");
// 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
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;
estimated_speed_filtered = estimated_speed_filtered * 0.9 + (float)estimated_speed * 0.1; // low pass filter on estimated speed
#if DEBUG==2
Serial.print(angle_adjusted);
Serial.print(" ");
Serial.println(estimated_speed_filtered);
#endif
if (positionControlMode)
{
// POSITION CONTROL. INPUT: Target steps for each motor. Output: motors speed
motor1_control = positionPDControl(steps1, target_steps1, Kp_position, Kd_position, speed_M1);
motor2_control = positionPDControl(steps2, target_steps2, Kp_position, Kd_position, speed_M2);
// Convert from motor position control to throttle / steering commands
throttle = (motor1_control + motor2_control) / 2;
throttle = constrain(throttle, -190, 190);
steering = motor2_control - motor1_control;
steering = constrain(steering, -50, 50);
}
// ROBOT SPEED CONTROL: This is a PI controller.
// input:user throttle(robot speed), variable: estimated robot speed, output: target robot angle to get the desired speed
target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
#if DEBUG==3
Serial.print(angle_adjusted);
Serial.print(" ");
Serial.print(estimated_speed_filtered);
Serial.print(" ");
Serial.println(target_angle);
#endif
// Stability control (100Hz loop): This is a PD controller.
// input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
// We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.
control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control
// The steering part from the user is injected directly to the output
motor1 = control_output + steering;
motor2 = control_output - steering;
// Limit max speed (control output)
motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
int angle_ready;
if (OSCpush[0]) // If we press the SERVO button we start to move
angle_ready = 80;
else
angle_ready = 74; // Default angle
if ((angle_adjusted < angle_ready) && (angle_adjusted > -angle_ready)) // Is robot ready (upright?)
{
// NORMAL MODE
digitalWrite(4, LOW); // Motors enable
// NOW we send the commands to the motors
setMotorSpeedM1(motor1);
setMotorSpeedM2(motor2);
}
else // Robot not ready (flat), angle > angle_ready => ROBOT OFF
{
digitalWrite(4, HIGH); // Disable motors
setMotorSpeedM1(0);
setMotorSpeedM2(0);
PID_errorSum = 0; // Reset PID I term
Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
Kd = KD_RAISEUP;
Kp_thr = KP_THROTTLE_RAISEUP;
Ki_thr = KI_THROTTLE_RAISEUP;
// RESET steps
steps1 = 0;
steps2 = 0;
}
// Push1 Move servo arm
if (OSCpush[0]) // Move arm
{
if (angle_adjusted > -40)
BROBOT_moveServo1(SERVO_MIN_PULSEWIDTH);
else
BROBOT_moveServo1(SERVO_MAX_PULSEWIDTH);
}
else
BROBOT_moveServo1(SERVO_AUX_NEUTRO);
// Normal condition?
if ((angle_adjusted < 56) && (angle_adjusted > -56))
{
Kp = Kp_user; // Default user control gains
Kd = Kd_user;
Kp_thr = Kp_thr_user;
Ki_thr = Ki_thr_user;
}
else // We are in the raise up procedure => we use special control parameters
{
Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
Kd = KD_RAISEUP;
Kp_thr = KP_THROTTLE_RAISEUP;
Ki_thr = KI_THROTTLE_RAISEUP;
}
} // End of new IMU data
// Medium loop 7.5Hz
if (loop_counter >= 15)
{
loop_counter = 0;
// Telemetry here?
#if TELEMETRY_ANGLE==1
char auxS[25];
int ang_out = constrain(int(angle_adjusted * 10),-900,900);
sprintf(auxS, "$tA,%+04d", ang_out);
Serial1.println(auxS);
#endif
#if TELEMETRY_DEBUG==1
char auxS[50];
sprintf(auxS, "$tD,%d,%d,%ld", int(angle_adjusted * 10), int(estimated_speed_filtered), steps1);
Serial1.println(auxS);
#endif
} // End of medium loop
else if (slow_loop_counter >= 100) // 1Hz
{
slow_loop_counter = 0;
// Read status
#if TELEMETRY_BATTERY==1
BatteryValue = (BatteryValue + BROBOT_readBattery(false)) / 2;
sendBattery_counter++;
if (sendBattery_counter >= 3) { //Every 3 seconds we send a message
sendBattery_counter = 0;
Serial.print("B");
Serial.println(BatteryValue);
char auxS[25];
sprintf(auxS, "$tB,%04d", BatteryValue);
Serial1.println(auxS);
}
#endif
} // End of slow loop
}

181
Arduino/BROBOT_EVO2/Control.ino Normal file
View File

@@ -0,0 +1,181 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// License: GPL v2
// Control functions (PID controls, Steppers control...)
// PD controller implementation(Proportional, derivative). DT in seconds
float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
{
float error;
float output;
error = setPoint - input;
// Kd is implemented in two parts
// The biggest one using only the input (sensor) part not the SetPoint input-input(t-1).
// And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1)
float Kd_setPoint = constrain((setPoint - setPointOld), -8, 8); // We limit the input part...
output = Kp * error + (Kd * Kd_setPoint - Kd * (input - PID_errorOld)) / DT;
//Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
//PID_errorOld2 = PID_errorOld;
PID_errorOld = input; // error for Kd is only the input component
setPointOld = setPoint;
return (output);
}
// PI controller implementation (Proportional, integral). DT in seconds
float speedPIControl(float DT, int16_t input, int16_t setPoint, float Kp, float Ki)
{
int16_t error;
float output;
error = setPoint - input;
PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
//Serial.println(PID_errorSum);
output = Kp * error + Ki * PID_errorSum * DT; // DT is in miliseconds...
return (output);
}
float positionPDControl(long actualPos, long setPointPos, float Kpp, float Kdp, int16_t speedM)
{
float output;
float P;
P = constrain(Kpp * float(setPointPos - actualPos), -115, 115); // Limit command
output = P + Kdp * float(speedM);
return (output);
}
// TIMER 1 : STEPPER MOTOR1 SPEED CONTROL
ISR(TIMER1_COMPA_vect)
{
if (dir_M1 == 0) // If we are not moving we dont generate a pulse
return;
// We generate 1us STEP pulse
SET(PORTE, 6); // STEP MOTOR 1
//delay_1us();
if (dir_M1 > 0)
steps1--;
else
steps1++;
CLR(PORTE, 6);
}
// TIMER 3 : STEPPER MOTOR2 SPEED CONTROL
ISR(TIMER3_COMPA_vect)
{
if (dir_M2 == 0) // If we are not moving we dont generate a pulse
return;
// We generate 1us STEP pulse
SET(PORTD, 6); // STEP MOTOR 2
//delay_1us();
if (dir_M2 > 0)
steps2--;
else
steps2++;
CLR(PORTD, 6);
}
// Set speed of Stepper Motor1
// tspeed could be positive or negative (reverse)
void setMotorSpeedM1(int16_t tspeed)
{
long timer_period;
int16_t speed;
// Limit max speed?
// WE LIMIT MAX ACCELERATION of the motors
if ((speed_M1 - tspeed) > MAX_ACCEL)
speed_M1 -= MAX_ACCEL;
else if ((speed_M1 - tspeed) < -MAX_ACCEL)
speed_M1 += MAX_ACCEL;
else
speed_M1 = tspeed;
#if MICROSTEPPING==16
speed = speed_M1 * 50; // Adjust factor from control output speed to real motor speed in steps/second
#else
speed = speed_M1 * 25; // 1/8 Microstepping
#endif
if (speed == 0)
{
timer_period = ZERO_SPEED;
dir_M1 = 0;
}
else if (speed > 0)
{
timer_period = 2000000 / speed; // 2Mhz timer
dir_M1 = 1;
SET(PORTB, 4); // DIR Motor 1 (Forward)
}
else
{
timer_period = 2000000 / -speed;
dir_M1 = -1;
CLR(PORTB, 4); // Dir Motor 1
}
if (timer_period > 65535) // Check for minimun speed (maximun period without overflow)
timer_period = ZERO_SPEED;
OCR1A = timer_period;
// Check if we need to reset the timer...
if (TCNT1 > OCR1A)
TCNT1 = 0;
}
// Set speed of Stepper Motor2
// tspeed could be positive or negative (reverse)
void setMotorSpeedM2(int16_t tspeed)
{
long timer_period;
int16_t speed;
// Limit max speed?
// WE LIMIT MAX ACCELERATION of the motors
if ((speed_M2 - tspeed) > MAX_ACCEL)
speed_M2 -= MAX_ACCEL;
else if ((speed_M2 - tspeed) < -MAX_ACCEL)
speed_M2 += MAX_ACCEL;
else
speed_M2 = tspeed;
#if MICROSTEPPING==16
speed = speed_M2 * 50; // Adjust factor from control output speed to real motor speed in steps/second
#else
speed = speed_M2 * 25; // 1/8 Microstepping
#endif
if (speed == 0)
{
timer_period = ZERO_SPEED;
dir_M2 = 0;
}
else if (speed > 0)
{
timer_period = 2000000 / speed; // 2Mhz timer
dir_M2 = 1;
CLR(PORTC, 6); // Dir Motor2 (Forward)
}
else
{
timer_period = 2000000 / -speed;
dir_M2 = -1;
SET(PORTC, 6); // DIR Motor 2
}
if (timer_period > 65535) // Check for minimun speed (maximun period without overflow)
timer_period = ZERO_SPEED;
OCR3A = timer_period;
// Check if we need to reset the timer...
if (TCNT3 > OCR3A)
TCNT3 = 0;
}

716
Arduino/BROBOT_EVO2/MPU6050.ino Normal file
View File

@@ -0,0 +1,716 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// License: GPL v2
// MPU6050 IMU code
// Read the accel and gyro registers and calculate a complementary filter for sensor fusion between gyros and accel
// Code based on arduino.cc MPU6050 sample
// Open Source / Public Domain
//
// Documentation:"MPU-6000 and MPU-6050 Register Map and Descriptions": RM-MPU-6000A.pdf
// MPU6050 Register map
#define MPU6050_AUX_VDDIO 0x01 // R/W
#define MPU6050_SMPLRT_DIV 0x19 // R/W
#define MPU6050_CONFIG 0x1A // R/W
#define MPU6050_GYRO_CONFIG 0x1B // R/W
#define MPU6050_ACCEL_CONFIG 0x1C // R/W
#define MPU6050_FF_THR 0x1D // R/W
#define MPU6050_FF_DUR 0x1E // R/W
#define MPU6050_MOT_THR 0x1F // R/W
#define MPU6050_MOT_DUR 0x20 // R/W
#define MPU6050_ZRMOT_THR 0x21 // R/W
#define MPU6050_ZRMOT_DUR 0x22 // R/W
#define MPU6050_FIFO_EN 0x23 // R/W
#define MPU6050_I2C_MST_CTRL 0x24 // R/W
#define MPU6050_I2C_SLV0_ADDR 0x25 // R/W
#define MPU6050_I2C_SLV0_REG 0x26 // R/W
#define MPU6050_I2C_SLV0_CTRL 0x27 // R/W
#define MPU6050_I2C_SLV1_ADDR 0x28 // R/W
#define MPU6050_I2C_SLV1_REG 0x29 // R/W
#define MPU6050_I2C_SLV1_CTRL 0x2A // R/W
#define MPU6050_I2C_SLV2_ADDR 0x2B // R/W
#define MPU6050_I2C_SLV2_REG 0x2C // R/W
#define MPU6050_I2C_SLV2_CTRL 0x2D // R/W
#define MPU6050_I2C_SLV3_ADDR 0x2E // R/W
#define MPU6050_I2C_SLV3_REG 0x2F // R/W
#define MPU6050_I2C_SLV3_CTRL 0x30 // R/W
#define MPU6050_I2C_SLV4_ADDR 0x31 // R/W
#define MPU6050_I2C_SLV4_REG 0x32 // R/W
#define MPU6050_I2C_SLV4_DO 0x33 // R/W
#define MPU6050_I2C_SLV4_CTRL 0x34 // R/W
#define MPU6050_I2C_SLV4_DI 0x35 // R
#define MPU6050_I2C_MST_STATUS 0x36 // R
#define MPU6050_INT_PIN_CFG 0x37 // R/W
#define MPU6050_INT_ENABLE 0x38 // R/W
#define MPU6050_INT_STATUS 0x3A // R
#define MPU6050_ACCEL_XOUT_H 0x3B // R
#define MPU6050_ACCEL_XOUT_L 0x3C // R
#define MPU6050_ACCEL_YOUT_H 0x3D // R
#define MPU6050_ACCEL_YOUT_L 0x3E // R
#define MPU6050_ACCEL_ZOUT_H 0x3F // R
#define MPU6050_ACCEL_ZOUT_L 0x40 // R
#define MPU6050_TEMP_OUT_H 0x41 // R
#define MPU6050_TEMP_OUT_L 0x42 // R
#define MPU6050_GYRO_XOUT_H 0x43 // R
#define MPU6050_GYRO_XOUT_L 0x44 // R
#define MPU6050_GYRO_YOUT_H 0x45 // R
#define MPU6050_GYRO_YOUT_L 0x46 // R
#define MPU6050_GYRO_ZOUT_H 0x47 // R
#define MPU6050_GYRO_ZOUT_L 0x48 // R
#define MPU6050_EXT_SENS_DATA_00 0x49 // R
#define MPU6050_EXT_SENS_DATA_01 0x4A // R
#define MPU6050_EXT_SENS_DATA_02 0x4B // R
#define MPU6050_EXT_SENS_DATA_03 0x4C // R
#define MPU6050_EXT_SENS_DATA_04 0x4D // R
#define MPU6050_EXT_SENS_DATA_05 0x4E // R
#define MPU6050_EXT_SENS_DATA_06 0x4F // R
#define MPU6050_EXT_SENS_DATA_07 0x50 // R
#define MPU6050_EXT_SENS_DATA_08 0x51 // R
#define MPU6050_EXT_SENS_DATA_09 0x52 // R
#define MPU6050_EXT_SENS_DATA_10 0x53 // R
#define MPU6050_EXT_SENS_DATA_11 0x54 // R
#define MPU6050_EXT_SENS_DATA_12 0x55 // R
#define MPU6050_EXT_SENS_DATA_13 0x56 // R
#define MPU6050_EXT_SENS_DATA_14 0x57 // R
#define MPU6050_EXT_SENS_DATA_15 0x58 // R
#define MPU6050_EXT_SENS_DATA_16 0x59 // R
#define MPU6050_EXT_SENS_DATA_17 0x5A // R
#define MPU6050_EXT_SENS_DATA_18 0x5B // R
#define MPU6050_EXT_SENS_DATA_19 0x5C // R
#define MPU6050_EXT_SENS_DATA_20 0x5D // R
#define MPU6050_EXT_SENS_DATA_21 0x5E // R
#define MPU6050_EXT_SENS_DATA_22 0x5F // R
#define MPU6050_EXT_SENS_DATA_23 0x60 // R
#define MPU6050_MOT_DETECT_STATUS 0x61 // R
#define MPU6050_I2C_SLV0_DO 0x63 // R/W
#define MPU6050_I2C_SLV1_DO 0x64 // R/W
#define MPU6050_I2C_SLV2_DO 0x65 // R/W
#define MPU6050_I2C_SLV3_DO 0x66 // R/W
#define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W
#define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W
#define MPU6050_MOT_DETECT_CTRL 0x69 // R/W
#define MPU6050_USER_CTRL 0x6A // R/W
#define MPU6050_PWR_MGMT_1 0x6B // R/W
#define MPU6050_PWR_MGMT_2 0x6C // R/W
#define MPU6050_FIFO_COUNTH 0x72 // R/W
#define MPU6050_FIFO_COUNTL 0x73 // R/W
#define MPU6050_FIFO_R_W 0x74 // R/W
#define MPU6050_WHO_AM_I 0x75 // R
// Defines for the bits, to be able to change
// between bit number and binary definition.
// By using the bit number, programming the sensor
// is like programming the AVR microcontroller.
// But instead of using "(1<<X)", or "_BV(X)",
// the Arduino "bit(X)" is used.
#define MPU6050_D0 0
#define MPU6050_D1 1
#define MPU6050_D2 2
#define MPU6050_D3 3
#define MPU6050_D4 4
#define MPU6050_D5 5
#define MPU6050_D6 6
#define MPU6050_D7 7
// AUX_VDDIO Register
#define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC
// CONFIG Register
// DLPF is Digital Low Pass Filter for both gyro and accelerometers.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DLPF_CFG0 MPU6050_D0
#define MPU6050_DLPF_CFG1 MPU6050_D1
#define MPU6050_DLPF_CFG2 MPU6050_D2
#define MPU6050_EXT_SYNC_SET0 MPU6050_D3
#define MPU6050_EXT_SYNC_SET1 MPU6050_D4
#define MPU6050_EXT_SYNC_SET2 MPU6050_D5
// Combined definitions for the EXT_SYNC_SET values
#define MPU6050_EXT_SYNC_SET_0 (0)
#define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1))
#define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2))
#define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0))
#define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1))
#define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0))
// Alternative names for the combined definitions.
#define MPU6050_EXT_SYNC_DISABLED MPU6050_EXT_SYNC_SET_0
#define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7
// Combined definitions for the DLPF_CFG values
#define MPU6050_DLPF_CFG_0 (0)
#define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1))
#define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2))
#define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0))
#define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1))
#define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0))
// Alternative names for the combined definitions
// This name uses the bandwidth (Hz) for the accelometer,
// for the gyro the bandwidth is almost the same.
#define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0
#define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1
#define MPU6050_DLPF_94HZ MPU6050_DLPF_CFG_2
#define MPU6050_DLPF_44HZ MPU6050_DLPF_CFG_3
#define MPU6050_DLPF_21HZ MPU6050_DLPF_CFG_4
#define MPU6050_DLPF_10HZ MPU6050_DLPF_CFG_5
#define MPU6050_DLPF_5HZ MPU6050_DLPF_CFG_6
#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7
// GYRO_CONFIG Register
// The XG_ST, YG_ST, ZG_ST are bits for selftest.
// The FS_SEL sets the range for the gyro.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_FS_SEL0 MPU6050_D3
#define MPU6050_FS_SEL1 MPU6050_D4
#define MPU6050_ZG_ST MPU6050_D5
#define MPU6050_YG_ST MPU6050_D6
#define MPU6050_XG_ST MPU6050_D7
// Combined definitions for the FS_SEL values
#define MPU6050_FS_SEL_0 (0)
#define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0))
#define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1))
#define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0))
// Alternative names for the combined definitions
// The name uses the range in degrees per second.
#define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0
#define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1
#define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2
#define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3
// ACCEL_CONFIG Register
// The XA_ST, YA_ST, ZA_ST are bits for selftest.
// The AFS_SEL sets the range for the accelerometer.
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_ACCEL_HPF0 MPU6050_D0
#define MPU6050_ACCEL_HPF1 MPU6050_D1
#define MPU6050_ACCEL_HPF2 MPU6050_D2
#define MPU6050_AFS_SEL0 MPU6050_D3
#define MPU6050_AFS_SEL1 MPU6050_D4
#define MPU6050_ZA_ST MPU6050_D5
#define MPU6050_YA_ST MPU6050_D6
#define MPU6050_XA_ST MPU6050_D7
// Combined definitions for the ACCEL_HPF values
#define MPU6050_ACCEL_HPF_0 (0)
#define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0))
#define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1))
#define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
#define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2))
#define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0))
// Alternative names for the combined definitions
// The name uses the Cut-off frequency.
#define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0
#define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1
#define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2
#define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3
#define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4
#define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7
// Combined definitions for the AFS_SEL values
#define MPU6050_AFS_SEL_0 (0)
#define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0))
#define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1))
#define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0))
// Alternative names for the combined definitions
// The name uses the full scale range for the accelerometer.
#define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0
#define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1
#define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2
#define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3
// FIFO_EN Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_SLV0_FIFO_EN MPU6050_D0
#define MPU6050_SLV1_FIFO_EN MPU6050_D1
#define MPU6050_SLV2_FIFO_EN MPU6050_D2
#define MPU6050_ACCEL_FIFO_EN MPU6050_D3
#define MPU6050_ZG_FIFO_EN MPU6050_D4
#define MPU6050_YG_FIFO_EN MPU6050_D5
#define MPU6050_XG_FIFO_EN MPU6050_D6
#define MPU6050_TEMP_FIFO_EN MPU6050_D7
// I2C_PIN_CFG Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_CLKOUT_EN MPU6050_D0
#define MPU6050_I2C_BYPASS_EN MPU6050_D1
#define MPU6050_FSYNC_INT_EN MPU6050_D2
#define MPU6050_FSYNC_INT_LEVEL MPU6050_D3
#define MPU6050_INT_RD_CLEAR MPU6050_D4
#define MPU6050_LATCH_INT_EN MPU6050_D5
#define MPU6050_INT_OPEN MPU6050_D6
#define MPU6050_INT_LEVEL MPU6050_D7
// INT_ENABLE Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DATA_RDY_EN MPU6050_D0
#define MPU6050_I2C_MST_INT_EN MPU6050_D3
#define MPU6050_FIFO_OFLOW_EN MPU6050_D4
#define MPU6050_ZMOT_EN MPU6050_D5
#define MPU6050_MOT_EN MPU6050_D6
#define MPU6050_FF_EN MPU6050_D7
// INT_STATUS Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_DATA_RDY_INT MPU6050_D0
#define MPU6050_I2C_MST_INT MPU6050_D3
#define MPU6050_FIFO_OFLOW_INT MPU6050_D4
#define MPU6050_ZMOT_INT MPU6050_D5
#define MPU6050_MOT_INT MPU6050_D6
#define MPU6050_FF_INT MPU6050_D7
// MOT_DETECT_STATUS Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_MOT_ZRMOT MPU6050_D0
#define MPU6050_MOT_ZPOS MPU6050_D2
#define MPU6050_MOT_ZNEG MPU6050_D3
#define MPU6050_MOT_YPOS MPU6050_D4
#define MPU6050_MOT_YNEG MPU6050_D5
#define MPU6050_MOT_XPOS MPU6050_D6
#define MPU6050_MOT_XNEG MPU6050_D7
// IC2_MST_DELAY_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0
#define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1
#define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2
#define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3
#define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4
#define MPU6050_DELAY_ES_SHADOW MPU6050_D7
// SIGNAL_PATH_RESET Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_TEMP_RESET MPU6050_D0
#define MPU6050_ACCEL_RESET MPU6050_D1
#define MPU6050_GYRO_RESET MPU6050_D2
// MOT_DETECT_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_MOT_COUNT0 MPU6050_D0
#define MPU6050_MOT_COUNT1 MPU6050_D1
#define MPU6050_FF_COUNT0 MPU6050_D2
#define MPU6050_FF_COUNT1 MPU6050_D3
#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4
#define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5
// Combined definitions for the MOT_COUNT
#define MPU6050_MOT_COUNT_0 (0)
#define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0))
#define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1))
#define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0))
// Alternative names for the combined definitions
#define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0
// Combined definitions for the FF_COUNT
#define MPU6050_FF_COUNT_0 (0)
#define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0))
#define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1))
#define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0))
// Alternative names for the combined definitions
#define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0
// Combined definitions for the ACCEL_ON_DELAY
#define MPU6050_ACCEL_ON_DELAY_0 (0)
#define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0))
#define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1))
#define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0))
// Alternative names for the ACCEL_ON_DELAY
#define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0
#define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1
#define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2
#define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3
// USER_CTRL Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_SIG_COND_RESET MPU6050_D0
#define MPU6050_I2C_MST_RESET MPU6050_D1
#define MPU6050_FIFO_RESET MPU6050_D2
#define MPU6050_I2C_IF_DIS MPU6050_D4 // must be 0 for MPU-6050
#define MPU6050_I2C_MST_EN MPU6050_D5
#define MPU6050_FIFO_EN MPU6050_D6
// PWR_MGMT_1 Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_CLKSEL0 MPU6050_D0
#define MPU6050_CLKSEL1 MPU6050_D1
#define MPU6050_CLKSEL2 MPU6050_D2
#define MPU6050_TEMP_DIS MPU6050_D3 // 1: disable temperature sensor
#define MPU6050_CYCLE MPU6050_D5 // 1: sample and sleep
#define MPU6050_SLEEP MPU6050_D6 // 1: sleep mode
#define MPU6050_DEVICE_RESET MPU6050_D7 // 1: reset to default values
// Combined definitions for the CLKSEL
#define MPU6050_CLKSEL_0 (0)
#define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1))
#define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2))
#define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0))
#define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1))
#define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0))
// Alternative names for the combined definitions
#define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0
#define MPU6050_CLKSEL_X MPU6050_CLKSEL_1
#define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2
#define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3
#define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4
#define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5
#define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6
#define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7
// PWR_MGMT_2 Register
// These are the names for the bits.
// Use these only with the bit() macro.
#define MPU6050_STBY_ZG MPU6050_D0
#define MPU6050_STBY_YG MPU6050_D1
#define MPU6050_STBY_XG MPU6050_D2
#define MPU6050_STBY_ZA MPU6050_D3
#define MPU6050_STBY_YA MPU6050_D4
#define MPU6050_STBY_XA MPU6050_D5
#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6
#define MPU6050_LP_WAKE_CTRL1 MPU6050_D7
// Combined definitions for the LP_WAKE_CTRL
#define MPU6050_LP_WAKE_CTRL_0 (0)
#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0))
#define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1))
#define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0))
// Alternative names for the combined definitions
// The names uses the Wake-up Frequency.
#define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0
#define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1
#define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2
#define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3
// Default I2C address for the MPU-6050 is 0x68.
#define MPU6050_I2C_ADDRESS 0x68
// Util function to swap byte values
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
// Declaring an union for the registers and the axis values.
// The byte order does not match the byte order of
// the compiler and AVR chip.
// The AVR chip (on the Arduino board) has the Low Byte
// at the lower address.
// But the MPU-6050 has a different order: High Byte at
// lower address, so that has to be corrected.
// The register part "reg" is only used internally,
// and are swapped in code.
typedef union accel_t_gyro_union
{
struct
{
uint8_t x_accel_h;
uint8_t x_accel_l;
uint8_t y_accel_h;
uint8_t y_accel_l;
uint8_t z_accel_h;
uint8_t z_accel_l;
uint8_t t_h;
uint8_t t_l;
uint8_t x_gyro_h;
uint8_t x_gyro_l;
uint8_t y_gyro_h;
uint8_t y_gyro_l;
uint8_t z_gyro_h;
uint8_t z_gyro_l;
} reg;
struct
{
int16_t x_accel;
int16_t y_accel;
int16_t z_accel;
int16_t temperature;
int16_t x_gyro;
int16_t y_gyro;
int16_t z_gyro;
} value;
};
// Global MPU6050 IMU variables
accel_t_gyro_union accel_t_gyro;
float x_gyro_value; // in deg/seg units
float x_gyro_offset = 0.0;
float accel_angle; // in degree units
float angle;
// This function implements a complementary filter to fusion gyro and accel info
float MPU6050_getAngle(float dt)
{
accel_angle = atan2f((float)accel_t_gyro.value.y_accel, (float)accel_t_gyro.value.z_accel) * RAD2GRAD;
x_gyro_value = (accel_t_gyro.value.x_gyro - x_gyro_offset) / 65.5; // Accel scale at 500deg/seg => 65.5 LSB/deg/s
// Complementary filter
// We integrate the gyro rate value to obtain the angle in the short term and we take the accelerometer angle with a low pass filter in the long term...
angle = 0.99 * (angle + x_gyro_value * dt) + 0.01 * accel_angle; // Time constant = 0.99*0.01(100hz)/(1-0.99) = 0.99, around 1 sec.
// Gyro bias correction
// We supose that the long term mean of the gyro_value should tend to zero (gyro_offset). This means that the robot is not continuosly rotating.
int16_t correction = constrain(accel_t_gyro.value.x_gyro, x_gyro_offset - 10, x_gyro_offset + 10); // limit corrections...
x_gyro_offset = x_gyro_offset * 0.9995 + correction * 0.0005; // Time constant of this correction is around 20 sec.
//Serial.print(angle);
//Serial.print(" ");
//Serial.println(x_gyro_offset);
return angle;
}
// Calibrate function. Take 100 readings (over 2 seconds) to calculate the gyro offset value. IMU should be steady in this process...
void MPU6050_calibrate()
{
int i;
long value = 0;
float dev;
int16_t values[100];
bool gyro_cal_ok = false;
delay(500);
while (!gyro_cal_ok){
Serial.println("Gyro calibration... DONT MOVE!");
// we take 100 measurements in 4 seconds
for (i = 0; i < 100; i++)
{
MPU6050_read_3axis();
values[i] = accel_t_gyro.value.x_gyro;
value += accel_t_gyro.value.x_gyro;
delay(25);
}
// mean value
value = value / 100;
// calculate the standard deviation
dev = 0;
for (i = 0; i < 100; i++)
dev += (values[i] - value) * (values[i] - value);
dev = sqrt((1 / 100.0) * dev);
Serial.print("offset: ");
Serial.print(value);
Serial.print(" stddev: ");
Serial.println(dev);
if (dev < 50.0)
gyro_cal_ok = true;
else
Serial.println("Repeat, DONT MOVE!");
}
x_gyro_offset = value;
// Take the first reading of angle from accels
angle = atan2f((float)accel_t_gyro.value.y_accel, (float)accel_t_gyro.value.z_accel) * RAD2GRAD;
}
void MPU6050_setup()
{
int error;
uint8_t c;
error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1);
Serial.print("WHO_AM_I : ");
Serial.print(c, HEX);
Serial.print(", error = ");
Serial.println(error, DEC);
// RESET chip
MPU6050_write_reg(MPU6050_PWR_MGMT_1, bit(MPU6050_DEVICE_RESET));
delay(125);
// Clear the 'sleep' bit to start the sensor and select clock source
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0x01);
//MPU6050_write_reg(MPU6050_PWR_MGMT_1,MPU6050_CLKSEL_Z);
// Config Gyro scale (500deg/seg)
MPU6050_write_reg(MPU6050_GYRO_CONFIG, MPU6050_FS_SEL_500);
// Config Accel scale (2g)
MPU6050_write_reg(MPU6050_ACCEL_CONFIG, MPU6050_AFS_SEL_2G);
// Config Digital Low Pass Filter 10Hz
MPU6050_write_reg(MPU6050_CONFIG, MPU6050_DLPF_10HZ);
// Set Sample Rate to 100Hz
MPU6050_write_reg(MPU6050_SMPLRT_DIV, 9); // 100Hz : Sample Rate = 1000 / (1 + SMPLRT_DIV) Hz
// Data ready interrupt enable
MPU6050_write_reg(MPU6050_INT_ENABLE, MPU6050_DATA_RDY_EN);
// Clear the 'sleep' bit to start the sensor (and select clock source).
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0x01);
// Clear the 'sleep' bit to start the sensor.
//MPU6050_write_reg(MPU6050_PWR_MGMT_1,MPU6050_CLKSEL_Z);
//MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
}
void MPU6050_read_3axis()
{
int error;
// read 14 bytes (gyros, temp and accels)
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
if (error != 0) {
//Serial.print("MPU6050 Error:");
//Serial.println(error);
}
// swap bytes
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
/*
// Print the raw acceleration values
Serial.print("ACC:");
Serial.print(accel_t_gyro.value.x_accel, DEC);
Serial.print(",");
Serial.print(accel_t_gyro.value.y_accel, DEC);
Serial.print(",");
Serial.print(accel_t_gyro.value.z_accel, DEC);
Serial.println();
// Print the raw gyro values.
Serial.print("GY:");
Serial.print(accel_t_gyro.value.x_gyro, DEC);
Serial.print(",");
Serial.print(accel_t_gyro.value.y_gyro, DEC);
Serial.print(",");
Serial.print(accel_t_gyro.value.z_gyro, DEC);
Serial.print(",");
Serial.println();
*/
}
void MPU6050_read_1axis()
{
int error;
// read X accel
error = MPU6050_read(MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro.reg.x_accel_h, 6);
if (error != 0) {
//Serial.print("MPU6050 Error:");
//Serial.println(error);
}
// read X gyro
error = MPU6050_read(MPU6050_GYRO_XOUT_H, (uint8_t *) &accel_t_gyro.reg.x_gyro_h, 2);
if (error != 0) {
//Serial.print("MPU6050 Error:");
//Serial.println(error);
}
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
// Print values
Serial.print("axis:");
Serial.print(accel_t_gyro.value.y_accel, DEC);
Serial.print(",");
Serial.println(accel_t_gyro.value.x_gyro, DEC);
}
// return true on new data available
bool MPU6050_newData()
{
uint8_t status;
int error;
error = MPU6050_read(MPU6050_INT_STATUS, &status, 1);
if (error != 0) {
//Serial.print("MPU6050 Error:");
//Serial.println(error);
}
if (status & (0b00000001)) // Data ready?
return true;
else
return false;
}
// MPU6050_read n bytes
int MPU6050_read(int start, uint8_t *buffer, int size)
{
int i, n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1)
return (-10);
n = Wire.endTransmission(false); // hold the I2C-bus
if (n != 0)
return (n);
// Third parameter is true: relase I2C-bus after data is read.
Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
i = 0;
while (Wire.available() && i < size)
{
buffer[i++] = Wire.read();
}
if ( i != size)
return (-11);
return (0); // return : no error
}
// MPU6050_write n bytes
int MPU6050_write(int start, const uint8_t *pData, int size)
{
int n, error;
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
n = Wire.write(start); // write the start address
if (n != 1)
return (-20);
n = Wire.write(pData, size); // write data bytes
if (n != size)
return (-21);
error = Wire.endTransmission(true); // release the I2C-bus
if (error != 0)
return (error);
return (0); // return : no error
}
// --------------------------------------------------------
// MPU6050_write_reg (only 1 byte)
int MPU6050_write_reg(int reg, uint8_t data)
{
int error;
error = MPU6050_write(reg, &data, 1);
return (error);
}

144
Arduino/BROBOT_EVO2/Network.ino Normal file
View File

@@ -0,0 +1,144 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// License: GPL v2
// Network functions (ESP module)
// Read control PID parameters from user. This is only for advanced users that want to "play" with the controllers...
void readControlParameters()
{
// Parameters Mode (page2 controls)
// Parameter initialization (first time we enter page2)
if (!modifing_control_parameters)
{
for (uint8_t i = 0; i < 4; i++)
OSCfader[i] = 0.5;
OSCtoggle[0] = 0;
modifing_control_parameters = true;
Serial1.println("$P2");
}
// User could adjust KP, KD, KP_THROTTLE and KI_THROTTLE (fadder1,2,3,4)
// Now we need to adjust all the parameters all the times because we dont know what parameter has been moved
Kp_user = KP * 2 * OSCfader[0];
Kd_user = KD * 2 * OSCfader[1];
Kp_thr_user = KP_THROTTLE * 2 * OSCfader[2];
Ki_thr_user = KI_THROTTLE * 2 * OSCfader[3];
// Send a special telemetry message with the new parameters
char auxS[50];
sprintf(auxS, "$tP,%d,%d,%d,%d", int(Kp_user * 1000), int(Kd_user * 1000), int(Kp_thr_user * 1000), int(Ki_thr_user * 1000));
Serial1.println(auxS);
#if DEBUG>0
Serial.print("Par: ");
Serial.print(Kp_user);
Serial.print(" ");
Serial.print(Kd_user);
Serial.print(" ");
Serial.print(Kp_thr_user);
Serial.print(" ");
Serial.println(Ki_thr_user);
#endif
// Kill robot => Sleep
while (OSCtoggle[0] == 1)
{
//Reset external parameters
PID_errorSum = 0;
timer_old = millis();
setMotorSpeedM1(0);
setMotorSpeedM2(0);
digitalWrite(4, HIGH); // Disable motors
OSC_MsgRead();
}
}
int ESPwait(String stopstr, int timeout_secs)
{
String response;
bool found = false;
char c;
long timer_init;
long timer;
timer_init = millis();
while (!found) {
timer = millis();
if (((timer - timer_init) / 1000) > timeout_secs) { // Timeout?
Serial.println("!Timeout!");
return 0; // timeout
}
if (Serial1.available()) {
c = Serial1.read();
Serial.print(c);
response += c;
if (response.endsWith(stopstr)) {
found = true;
delay(10);
Serial1.flush();
Serial.println();
}
} // end Serial1_available()
} // end while (!found)
return 1;
}
// getMacAddress from ESP wifi module
int ESPgetMac()
{
char c1, c2;
bool timeout = false;
long timer_init;
long timer;
uint8_t state = 0;
uint8_t index = 0;
MAC = "";
timer_init = millis();
while (!timeout) {
timer = millis();
if (((timer - timer_init) / 1000) > 5) // Timeout?
timeout = true;
if (Serial1.available()) {
c2 = c1;
c1 = Serial1.read();
Serial.print(c1);
switch (state) {
case 0:
if (c1 == ':')
state = 1;
break;
case 1:
if (c1 == '\r') {
MAC.toUpperCase();
state = 2;
}
else {
if ((c1 != '"') && (c1 != ':'))
MAC += c1; // Uppercase
}
break;
case 2:
if ((c2 == 'O') && (c1 == 'K')) {
Serial.println();
Serial1.flush();
return 1; // Ok
}
break;
} // end switch
} // Serial_available
} // while (!timeout)
Serial.println("!Timeout!");
Serial1.flush();
return -1; // timeout
}
int ESPsendCommand(char *command, String stopstr, int timeout_secs)
{
Serial1.println(command);
ESPwait(stopstr, timeout_secs);
delay(250);
}

304
Arduino/BROBOT_EVO2/OSC.ino Normal file
View File

@@ -0,0 +1,304 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// License: GPL v2
// OSC functions (OSC = Open Sound Control protocol)
// OSC Messages read: OSC: /page/command parameters
// FADER (1,2,3,4) Ex: /1/fader1 f, XXXX => lenght:20, Param: float (0.0-1.0)
// XY (1,2) Ex: /1/xy1 f,f, XXXXXXXX => length: 24 Params: float,float (0.0-1.0)
// PUSH (1,2,3,4) Ex: /1/push1 f, XXXX => length:20 Param: float
// TOGGLE (1,2,3,4) Ex: /1/toggle1 f, XXXX => length:20 Param: float
// MOVE Ex: /1/m XXXX XXXX XXXX => length:16 Params: speed, steps1, steps2 (all float)
//
// OSC Message send:
// string to send + param (float)[last 4 bytes]
// for DEBUG uncomment this lines...
//#define OSCDEBUG 3
char UDPBuffer[8]; // input message buffer
// OSC message internal variables
unsigned char OSCreadStatus;
unsigned char OSCreadCounter;
unsigned char OSCreadNumParams;
unsigned char OSCcommandType;
unsigned char OSCtouchMessage;
// ------- OSC functions -----------------------------------------
// Aux functions
float OSC_extractParamFloat(uint8_t pos) {
union {
unsigned char Buff[4];
float d;
} u;
u.Buff[0] = (unsigned char)UDPBuffer[pos];
u.Buff[1] = (unsigned char)UDPBuffer[pos + 1];
u.Buff[2] = (unsigned char)UDPBuffer[pos + 2];
u.Buff[3] = (unsigned char)UDPBuffer[pos + 3];
return (u.d);
}
int16_t OSC_extractParamInt(uint8_t pos) {
union {
unsigned char Buff[2];
int16_t d;
} u;
u.Buff[1] = (unsigned char)UDPBuffer[pos];
u.Buff[0] = (unsigned char)UDPBuffer[pos + 1];
return (u.d);
}
void OSC_init()
{
OSCreadStatus = 0;
OSCreadCounter = 0;
OSCreadNumParams = 0;
OSCcommandType = 0;
OSCfader[0] = 0.5;
OSCfader[1] = 0.5;
OSCfader[2] = 0.5;
OSCfader[3] = 0.5;
}
void OSC_MsgSend(char *c, unsigned char msgSize, float p)
{
uint8_t i;
union {
unsigned char Buff[4];
float d;
} u;
// We copy the param in the last 4 bytes
u.d = p;
c[msgSize - 4] = u.Buff[3];
c[msgSize - 3] = u.Buff[2];
c[msgSize - 2] = u.Buff[1];
c[msgSize - 1] = u.Buff[0];
for (i = 0; i < msgSize; i++)
{
Serial1.write((uint8_t)c[i]);
//Serial.write((uint8_t)c[i]);
}
}
void OSC_MsgRead()
{
uint8_t i;
uint8_t tmp;
float value;
float value2;
// New bytes available to process?
if (Serial1.available() > 0) {
//Serial.print("B:");
//Serial.println(Serial1_available());
// We rotate the Buffer (we could implement a ring buffer in future)
for (i = 7; i > 0; i--) {
UDPBuffer[i] = UDPBuffer[i - 1];
}
UDPBuffer[0] = Serial1.read();
Serial.print(UDPBuffer[0]);
#ifdef OSCDEBUG3
Serial.print(UDPBuffer[0]);
#endif
// We look for an OSC message start like /x/
if ((UDPBuffer[0] == '/') && (UDPBuffer[2] == '/') && ((UDPBuffer[1] == '1') || (UDPBuffer[1] == '2'))) {
if (OSCreadStatus == 0) {
OSCpage = UDPBuffer[1] - '0'; // Convert page to int
OSCreadStatus = 1;
OSCtouchMessage = 0;
//Serial.print("$");
#ifdef OSCDEBUG3
Serial.println();
#endif
}
else {
Serial.println("!ERR:osc");
OSCreadStatus = 1;
}
return;
} else if (OSCreadStatus == 1) { // looking for the message type
// Fadder /1/fader1 ,f xxxx
if ((UDPBuffer[3] == 'd') && (UDPBuffer[2] == 'e') && (UDPBuffer[1] == 'r')) {
OSCreadStatus = 2; // Message type detected
OSCreadCounter = 11; // Bytes to read the parameter
OSCreadNumParams = 1; // 1 parameters
OSCcommandType = UDPBuffer[0] - '0';
#ifdef OSCDEBUG2
Serial.print("$FAD1");
Serial.print(OSCcommandType);
Serial.print("$");
#endif
return;
} // end fadder
// MOVE message
if ((UDPBuffer[3] == 'o') && (UDPBuffer[2] == 'v') && (UDPBuffer[1] == 'e')) {
OSCreadStatus = 2; // Message type detected
OSCreadCounter = 8; // Bytes to read the parameters
OSCreadNumParams = 3; // 3 parameters
OSCcommandType = 40;
#ifdef OSCDEBUG2
Serial.print("$MOVE:");
#endif
return;
} // End MOVE message
// XY message
if ((UDPBuffer[2] == 'x') && (UDPBuffer[1] == 'y')) {
OSCreadStatus = 2; // Message type detected
OSCreadCounter = 14; // Bytes to read the parameters
OSCreadNumParams = 2; // 2 parameters
OSCcommandType = 10 + (UDPBuffer[0] - '0');
return;
} // End XY message
// Push message
if ((UDPBuffer[3] == 'u') && (UDPBuffer[2] == 's') && (UDPBuffer[1] == 'h')) {
OSCreadStatus = 2; // Message type detected
OSCreadCounter = 10; // Bytes to read the parameter
OSCreadNumParams = 1; // 1 parameters
OSCcommandType = 20 + (UDPBuffer[0] - '1');
//Serial.println(commandType);
#ifdef OSCDEBUG2
Serial.print("$P"):
Serial.print(UDPBuffer[0] - '1');
Serial.print(":");
#endif
return;
} // end push
// Toggle message
if ((UDPBuffer[3] == 'g') && (UDPBuffer[2] == 'l') && (UDPBuffer[1] == 'e')) {
OSCreadStatus = 2; // Message type detected
OSCreadCounter = 10; // Bytes to read the parameter
OSCreadNumParams = 1; // 1 parameters
OSCcommandType = 30 + (UDPBuffer[0] - '1');
//Serial.println(commandType);
#ifdef OSCDEBUG2
Serial.print("$T"):
Serial.print(UDPBuffer[0] - '1');
Serial.print(":");
#endif
return;
} // end toggle
} else if (OSCreadStatus == 2) {
if ((UDPBuffer[1] == '/') && (UDPBuffer[0] == 'z')) { // Touch up message? (/z) [only on page1]
if ((OSCpage == 1) && (OSCcommandType <= 2)) { // Touchup message only on Fadder1 and Fadder2
OSCtouchMessage = 1;
}
else {
OSCtouchMessage = 0;
OSCreadStatus = 0; //Finish
}
} // Touch message(/z)
OSCreadCounter--; // Reading counter until we reach the Parameter position
if (OSCreadCounter <= 0) {
OSCreadStatus = 0;
OSCnewMessage = 1;
//Serial.println(value);
switch (OSCcommandType) {
case 1:
value = OSC_extractParamFloat(0);
OSCfader[0] = value;
if ((OSCtouchMessage) && (value == 0)) {
OSCfader[0] = 0.5;
//Serial.println("TOUCH_X");
OSC_MsgSend("/1/fader1\0\0\0,f\0\0\0\0\0\0", 20, 0.5);
}
#ifdef OSCDEBUG
Serial.print("$F1:");
Serial.println(OSCfadder[0]);
#endif
break;
case 2:
value = OSC_extractParamFloat(0);
OSCfader[1] = value;
if ((OSCtouchMessage) && (value == 0)) {
OSCfader[1] = 0.5;
//Serial.println("TOUCH_Y");
OSC_MsgSend("/1/fader2\0\0\0,f\0\0\0\0\0\0", 20, 0.5);
}
#ifdef OSCDEBUG
Serial.print("$F2:");
Serial.println(OSCfadder[1]);
#endif
break;
case 3:
OSCfader[2] = OSC_extractParamFloat(0);
#ifdef OSCDEBUG
Serial.print("$F3:");
Serial.println(OSCfadder[2]);
#endif
break;
case 4:
OSCfader[3] = OSC_extractParamFloat(0);
#ifdef OSCDEBUG
Serial.print("$F4:");
Serial.println(OSCfadder[3]);
#endif
break;
case 11:
OSCxy1_x = OSC_extractParamFloat(0);
OSCxy1_y = OSC_extractParamFloat(4);
#ifdef OSCDEBUG
Serial.print("$XY1:");
Serial.print(OSCxy1_x);
Serial.print(",");
Serial.println(OSCxy1_y);
#endif
break;
case 12:
OSCxy2_x = OSC_extractParamFloat(0);
OSCxy2_y = OSC_extractParamFloat(4);
#ifdef OSCDEBUG
Serial.print("$XY2:");
Serial.print(OSCxy2_x);
Serial.print(",");
Serial.println(OSCxy2_y);
#endif
break;
case 40:
// MOVE
OSCmove_mode = 1;
OSCmove_speed = OSC_extractParamInt(4);
OSCmove_steps1 = OSC_extractParamInt(2);
OSCmove_steps2 = OSC_extractParamInt(0);
#ifdef OSCDEBUG
Serial.print("$MOVE:");
Serial.print(OSCmove_speed);
Serial.print(",");
Serial.print(OSCmove_steps1);
Serial.print(",");
Serial.println(OSCmove_steps2);
#endif
break;
default:
// Push y toggle
value = OSC_extractParamFloat(0);
if ((OSCcommandType >= 20) && (OSCcommandType < 25))
{
if (value == 0)
OSCpush[OSCcommandType - 20] = 0;
else
OSCpush[OSCcommandType - 20] = 1;
}
if ((OSCcommandType >= 30) && (OSCcommandType < 35))
{
if (value == 0)
OSCtoggle[OSCcommandType - 30] = 0;
else
OSCtoggle[OSCcommandType - 30] = 1;
}
break;
} // switch
} // if (OSCRead_counter<=0)
} // if (OSCread_status==2)
} // end Serial.available()
}

87
Arduino/BROBOT_EVO2/Servos.ino Normal file
View File

@@ -0,0 +1,87 @@
// BROBOT EVO 2 by JJROBOTS
// SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS
// License: GPL v2
// Servo and aux functions
//#include "Arduino.h"
// Default servo definitions
#define SERVO1_AUX_NEUTRO 1500 // Servo neutral position
#define SERVO1_MIN_PULSEWIDTH 700
#define SERVO1_MAX_PULSEWIDTH 2300
#define SERVO2_AUX_NEUTRO 1500 // Servo neutral position
#define SERVO2_MIN_PULSEWIDTH 700
#define SERVO2_MAX_PULSEWIDTH 2300
#define BATT_VOLT_FACTOR 8
int battery;
// Init servo on T4 timer. Output OC4B (Leonardo Pin10)
// We configure the Timer4 for 11 bits PWM (enhacend precision) and 16.3ms period (OK for most servos)
// Resolution: 8us per step (this is OK for servos, around 175 steps for typical servo)
void BROBOT_initServo()
{
int temp;
// Initialize Timer4 as Fast PWM
TCCR4A = (1<<PWM4A)|(1<<PWM4B);
TCCR4B = 0;
TCCR4C = (1<<PWM4D);
TCCR4D = 0;
TCCR4E = (1<<ENHC4); // Enhaced -> 11 bits
temp = 1500>>3;
TC4H = temp >> 8;
OCR4B = temp & 0xff;
// Reset timer
TC4H = 0;
TCNT4 = 0;
// Set TOP to 1023 (10 bit timer)
TC4H = 3;
OCR4C = 0xFF;
// OC4A = PC7 (Pin13) OC4B = PB6 (Pin10) OC4D = PD7 (Pin6)
// Set pins as outputs
DDRB |= (1 << 6); // OC4B = PB6 (Pin10 on Leonardo board)
DDRC |= (1 << 7); // OC4A = PC7 (Pin13 on Leonardo board)
DDRD |= (1 << 7); // OC4D = PD7 (Pin6 on Leonardo board)
//Enable OC4A and OC4B and OCR4D output
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);
}
void BROBOT_moveServo1(int pwm)
{
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;
OCR4B = pwm & 0xFF;
}
void BROBOT_moveServo2(int pwm)
{
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;
OCR4A = pwm & 0xFF; // Old 2.0 boards servo2 output
OCR4D = pwm & 0xFF; // New 2.1 boards servo2 output
}
// output : Battery voltage*10 (aprox) and noise filtered
int BROBOT_readBattery(bool first_time)
{
if (first_time)
battery = analogRead(5)/BATT_VOLT_FACTOR;
else
battery = (battery*9 + (analogRead(5)/BATT_VOLT_FACTOR))/10;
return battery;
}

6404
Hardware/electronics/brobot19.sch Normal file
View File

File diff suppressed because it is too large Load Diff