Ståndpunkten uppskattning av en hjulförsedda bot med arduino. (4 / 5 steg)

Steg 4: Låt oss gå till denna position du lite bot!

Så, du har en robot som vet var han är men du måste kontrollera alla hans rörelser. Inte särskilt bra!
Låt oss fixa det genom att styra roboten endast med waypoints, som är mycket roligare.

Det gör behöver du inte någon annan mekanik eller elektronik! Det är ren mjukvara ;)

Låt oss se koden (det är lite lång och inte omfattande vid första anblicken:

/*
* ----------------------------------------------------------------------------
* "Den öl-WARE licens" (revidering 42):
* JBot skrev denna fil. Så länge du behåller detta märker du
* kan göra vad du vill med det här. Om vi träffas någon dag, och du tror
* Det här är värt det, du kan köpa mig en öl i gengäld.
* ----------------------------------------------------------------------------
*/

Annat omfattar
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >

/***********/
/ * Definieras * /
/***********/
#define TICK_PER_MM_LEFT 9.2628378129 / / 90.9456817668
#define TICK_PER_MM_RIGHT 9.2628378129 / / 90.9456817668
#define DIAMETER 270.4 //275.0 / / 166.0 / / avstånd mellan 2 hjul

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radianer till grader konvertering * /

Typer av motor
#define ALPHA_MOTOR 0
#define DELTA_MOTOR 1
#define LEFT_MOTOR 2
#define RIGHT_MOTOR 3

#define ALPHADELTA 0
#define vänster/höger 1

#define COMMAND_DONE 0
#define PROCESSING_COMMAND 1
#define WAITING_BEGIN 2
#define fel 3

#define ALPHA_MAX_SPEED 25000 //24000 //25000//13000 / / 9000
#define ALPHA_MAX_ACCEL 300
#define ALPHA_MAX_DECEL 3500 //2500
#define DELTA_MAX_SPEED 45000 //45000 //50000//37000
#define DELTA_MAX_SPEED_BACK 35000 //45000 //50000//37000
#define DELTA_MAX_SPEED_BACK_PAWN 45000
#define DELTA_MAX_ACCEL 1000 //900 //600
#define DELTA_MAX_DECEL 10000 //4000 //1800

/***********************/
/ * Särskilda strukturer * /
/***********************/
struct motor {
typen int;
signerade lång des_speed;
signerade lång cur_speed;
lång last_error;
lång error_sum;
int kP;
int kI;
int kD;
signerade lång accel;
undertecknad länge decel;
signerade lång max_speed;
dubbel avstånd;
};

struct robot {
dubbla pos_X;
dubbla pos_Y;
dubbel theta;
float gir;
float stigningen.
float rullen.
float yaw_offset;
};

struct RobotCommand {
char staten.
dubbla current_distance;
dubbla desired_distance;
};

struct punkt {
int x;
int y;
};

/********************/
/ * Globala variabler * /
/********************/
struct motor left_motor;
struct motor right_motor;
struct motor alpha_motor;
struct motor delta_motor;

struct robot maximus;

struct RobotCommand bot_command_delta;
struct RobotCommand prev_bot_command_delta;
struct RobotCommand bot_command_alpha;

flyktiga lång left_cnt = 0;
flyktiga lång right_cnt = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

Double total_distance = 0.0;

/***********************/
/ * AVBRYTA FUNKTIONER * /
/***********************/

Externa avbryta 4 rutin = > PIN2
ISR(INT4_vect)
{
om ((PINB & 0x10)! = 0) {
om ((PINE & 0x10)! = 0)
left_cnt--;
annat
left_cnt ++;
} annat {
om ((PINE & 0x10) == 0)
left_cnt--;
annat
left_cnt ++;
}

}

Externa avbryta 5 rutin = > PIN3
ISR(INT5_vect)
{
om ((PINK & 0x80)! = 0) {
om ((PINE & 0x20)! = 0)
right_cnt ++;
annat
right_cnt--;
} annat {
om ((PINE & 0x20) == 0)
right_cnt ++;
annat
right_cnt--;
}

}

PIN change 0-7 avbrottstjänstens rutin = > PIN10
ISR(PCINT0_vect)
{
om ((PINE & 0x10)! = 0) {
om ((PINB & 0x10)! = 0) {
left_cnt ++;
} annat
left_cnt--;
} annat {
om ((PINB & 0x10) == 0) {
left_cnt ++;
} annat
left_cnt--;
}

}

Ändra PIN 16-23 avbrottstjänstens rutin = > PIN-ADC15
ISR(PCINT2_vect)
{
om ((PINE & 0x20)! = 0) {
om ((PINK & 0x80)! = 0)
right_cnt--;
annat
right_cnt ++;
} annat {
om ((PINK & 0x80) == 0)
right_cnt--;
annat
right_cnt ++;
}

}

Timer 1 spill avbrottstjänstens rutin
ISR(TIMER1_OVF_vect)
{
SEI(); Aktivera avbrott
get_Odometers();

do_motion_control();
move_motors(ALPHADELTA); Uppdatera varvtal

}

/*************************/
/ * SYSTEM INITIALIZATION * /
/*************************/
void setup()
{

Input/Output Ports initiering
Port en initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTA = 0X00;
DDRA = 0X00;

Port B initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = ut
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTB = 0X00;
DDRB = 0X00;

Port C initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTC = 0X00;
DDRC = 0X00;

Port D initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTD = 0X00;
DDRD = 0X00;

Port E initieringen
Func2 = i Func1 = i Func0 = i
State2 = T State1 = T State0 = T
PORTE = 0X00;
DDRE = 0X00;

PORTK = 0X00;
DDRK = 0X00;

pinMode (13, OUTPUT);

Timer/Counter 1 initiering
Klocka Källa: systemklockan
Klockan värde: 62,500 kHz
Läge: Ph. rätt PWM topp = 00FFh
OC1A utgång: apparaten.
OC1B utgång: apparaten.
OC1C utgång: apparaten.
Brus Canceler: Off
Input Capture på fallande kant
Timer 1 spill avbryta: på
Input Capture avbrott: Off
Jämföra en Match avbryta: Off
Jämför B Match avbrott: Off
Jämföra C Match avbrott: Off
TCCR1A = 0X01;
TCCR1B = 0X04;
TCNT1H = 0X00;
TCNT1L = 0X00;
ICR1H = 0X00;
ICR1L = 0X00;
OCR1AH = 0X00;
OCR1AL = 0X00;
OCR1BH = 0X00;
OCR1BL = 0X00;
OCR1CH = 0X00;
OCR1CL = 0X00;

Externa Interrupt(s) initiering
EICRA = 0X00;
EICRB = 0X05;
EIMSK = 0X30;
EIFR = 0X30;
Avbryta på PCINT
PCICR = 0X05;
PCIFR = 0X05;
PCMSK0 = 0X10;
PCMSK1 = 0X00;
PCMSK2 = 0X80;

Timer(s) / Counter(s) Interrupt(s) initiering
TIMSK1 | = 0X01;
TIFR1 | = 0X01;

/******************************/
/ * Initieringen av koden * /
/******************************/
init_motors(); Init motorer
init_Robot(&Maximus); Init robot status

init_Command(&bot_command_delta); Init robot kommando
init_Command(&bot_command_alpha); Init robot kommando
init_Command(&prev_bot_command_delta);

Globala aktivera avbrott
SEI();

Delay(10);

}

/******************/
/ * MAIN KOD LOOP * /
/******************/
void loop()
{
Placera din kod här

Delay(20);

}

/****************************/
/ * INITIERA FUNKTIONER * /
/****************************/
void init_Robot (struct robot * my_robot)
{
my_robot -> pos_X = 0;
my_robot -> pos_Y = 0;
my_robot -> theta = 0;
my_robot -> gir = 0,0;
my_robot -> pitch = 0,0;
my_robot -> rulle = 0,0;
my_robot -> yaw_offset = 0,0;
}

void init_Command (struct RobotCommand * cmd)
{
CMD -> state = COMMAND_DONE;
CMD -> current_distance = 0;
CMD -> desired_distance = 0;
}

void init_motors(void)
{
/ * Vänster motor initiering * /
left_motor.type = LEFT_MOTOR;
left_motor.des_speed = 0;
left_motor.cur_speed = 0;
left_motor.last_error = 0;
left_motor.error_sum = 0;
left_motor.kP = 12;
left_motor.KI = 6;
left_motor.KD = 1;
left_motor.accel = 5;
left_motor.decel = 5;
left_motor.max_speed = 30.
left_motor.Distance = 0,0;

/ * Rätt motor initiering * /
right_motor.type = RIGHT_MOTOR;
right_motor.des_speed = 0;
right_motor.cur_speed = 0;
right_motor.last_error = 0;
right_motor.error_sum = 0;
right_motor.kP = 12;
right_motor.KI = 6;
right_motor.KD = 1;
right_motor.accel = 5;
right_motor.decel = 5;
right_motor.max_speed = 30.
right_motor.Distance = 0,0;

/ * Alpha motor initiering * /
alpha_motor.type = ALPHA_MOTOR;
alpha_motor.des_speed = 0;
alpha_motor.cur_speed = 0;
alpha_motor.last_error = 0;
alpha_motor.error_sum = 0;
alpha_motor.kP = 230; 250 / / 350 / / 600
alpha_motor.KI = 0;
alpha_motor.KD = 340; 300 //180 / / 200
alpha_motor.accel = ALPHA_MAX_ACCEL; 300; 350 / / 200; 300
alpha_motor.decel = ALPHA_MAX_DECEL; 1300; 1200; //1100; //1200; 500
alpha_motor.max_speed = ALPHA_MAX_SPEED; 7000; 8000
alpha_motor.Distance = 0,0;

/ * Delta motor initiering * /
delta_motor.type = DELTA_MOTOR;
delta_motor.des_speed = 0;
delta_motor.cur_speed = 0;
delta_motor.last_error = 0;
delta_motor.error_sum = 0;
delta_motor.kP = 600; 600
delta_motor.KI = 0;
delta_motor.KD = 200. 100 * 1,09
delta_motor.accel = DELTA_MAX_ACCEL; 600; 400, //500;
delta_motor.decel = DELTA_MAX_DECEL; 1800; 1350; //1100; //1200;
delta_motor.max_speed = DELTA_MAX_SPEED; 25000; //35000;
delta_motor.Distance = 0,0;
}

/*******************************/
/ * MOTION CONTROL FUNKTIONER * /
/*******************************/
void do_motion_control(void)
{

PID vinkel
alpha_motor.des_speed = compute_position_PID (& bot_command_alpha & alpha_motor);

PID avstånd
om ((bot_command_alpha.state == WAITING_BEGIN) || (bot_command_alpha.State == PROCESSING_COMMAND)) {/ / Om alpha motor inte har avslutat sin rörelse

} annat {
om ((bot_command_delta.state! = PROCESSING_COMMAND) & & (prev_bot_command_delta.state == WAITING_BEGIN)) {
prev_bot_command_delta.State = PROCESSING_COMMAND;
set_new_command (& bot_command_delta, prev_bot_command_delta.desired_distance);
}

}
delta_motor.des_speed = compute_position_PID (& bot_command_delta & delta_motor);

}

void set_new_command (struct RobotCommand * cmd, långa avstånd)
{
CMD -> state = WAITING_BEGIN;
CMD -> current_distance = 0;
CMD -> desired_distance = avstånd;
}

lång compute_position_PID (struct RobotCommand * cmd, struct motor * used_motor)
{
långa P, I, D;
långa errDif, fela;
långa tmp = 0;

om (cmd -> staten == WAITING_BEGIN) {
CMD -> state = PROCESSING_COMMAND;
}

om (used_motor -> Skriv == ALPHA_MOTOR)
ERR = cmd -> desired_distance * 10 - cmd -> current_distance * 10 * RAD2DEG;
annat
ERR = cmd -> desired_distance - cmd -> current_distance;

used_motor -> error_sum += fela; Fel summa
om (used_motor -> error_sum > 10)
used_motor -> error_sum = 10;
om (used_motor -> error_sum < -10)
used_motor -> error_sum = -10;

errDif = err - used_motor -> last_error; Beräkna fel variationen

used_motor -> last_error = fela;

P = err * used_motor -> kP; Proportionnal
Jag = used_motor -> error_sum * used_motor -> kI; Integral
D = errDif * used_motor -> kD; Bearbetningar

tmp = (P + I + D);

om (abs(tmp) < abs (used_motor -> des_speed)) {/ / retardation
om (tmp > (used_motor -> des_speed + used_motor -> decel))
tmp = (used_motor -> des_speed + used_motor -> decel);
annars om (tmp < (used_motor -> des_speed - used_motor -> decel))
tmp = (used_motor -> des_speed - used_motor -> decel);
} annat {/ / Acceleration
om (tmp > (used_motor -> des_speed + used_motor -> accel))
tmp = (used_motor -> des_speed + used_motor -> accel);
annars om (tmp < (used_motor -> des_speed - used_motor -> accel))
tmp = (used_motor -> des_speed - used_motor -> accel);
}

om (tmp > (used_motor -> max_speed))
tmp = (used_motor -> max_speed).
om (tmp <-(used_motor -> max_speed))
tmp =-(used_motor -> max_speed).

om (used_motor -> Skriv == ALPHA_MOTOR) {
om ((cmd -> staten == PROCESSING_COMMAND) & & (abs(err) < 3)
& & (abs(errDif) < 3)) {/ / 2 före
CMD -> state = COMMAND_DONE;
}
} annat {
om ((cmd -> staten == PROCESSING_COMMAND) & & (abs(err) < 6)
& & (abs(errDif) < 5)) {/ / 2 före
CMD -> state = COMMAND_DONE;
}
}

returnera tmp;
}

Beräkna avståndet till gör till gå till (x, y)
dubbel distance_coord (struct robot * my_robot, dubbla x1, dubbel y1)
{
dubbel x = 0;
x = sqrt (pow (Folkesson (x1-my_robot -> pos_X), 2) + pow (Folkesson (y1 - my_robot -> pos_Y), 2));
återvändande x;
}

Beräkna vinkeln ska göra för att gå till (x, y)
dubbel angle_coord (struct robot * my_robot, dubbla x1, dubbel y1)
{
Double angletodo = 0;
om ((x1 < my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {
angletodo = -PI / 2 - ARCTAN (Folkesson ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else om ((x1 > my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {
angletodo = - ARCTAN (Folkesson ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else om ((x1 > my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {
angletodo = ARCTAN (Folkesson ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else om ((x1 < my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {
angletodo = PI / 2 + ARCTAN (Folkesson ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else om ((x1 < my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = -PI;
} else om ((x1 > my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = 0;
} else om ((x1 == my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {/ /
angletodo = -PI / 2;
} else om ((x1 == my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {/ /
angletodo = PI / 2;
} annat
angletodo = 0;

angletodo = angletodo - my_robot -> theta;

om (angletodo > PI)
angletodo = angletodo - 2 * PI;
om (angletodo < -PI)
angletodo = 2 * PI + angletodo;

återvända angletodo;
}

void goto_xy (double x, dubbel y)
{
dubbel ang, dist;

Ang = angle_coord (& maximus, x, y) * RAD2DEG;
set_new_command (& bot_command_alpha, ang);

dist = distance_coord (& maximus, x, y);
set_new_command (& prev_bot_command_delta, dist);
bot_command_delta.State = WAITING_BEGIN;

}

void goto_xy_back (double x, dubbel y)
{
dubbel ang, dist;

Ang = angle_coord (& maximus, x, y);
om (ang < 0)
Ang = (ang + PI) * RAD2DEG;
annat
Ang = (ang - PI) * RAD2DEG;
set_new_command (& bot_command_alpha, ang);

dist = - distance_coord (& maximus, x, y);
set_new_command (& prev_bot_command_delta, dist);
bot_command_delta.State = WAITING_BEGIN;
}

/********************/
/ * MOTORS FUNKTIONER * /
/********************/
void move_motors(char type)
{
om (typ == ALPHADELTA) {
write_RoboClaw_speed_M1M2 (128, delta_motor.des_speed - alpha_motor.des_speed, delta_motor.des_speed + alpha_motor.des_speed);
SÄTTA ENHETEN MOTOR KODEN HÄR
}
annat {
write_RoboClaw_speed_M1M2 (128, left_motor.des_speed, right_motor.des_speed);
SÄTTA ENHETEN MOTOR KODEN HÄR
}
}

void update_motor (struct motor * used_motor)
{
Växla (used_motor -> typ) {
fall LEFT_MOTOR:
used_motor -> cur_speed = left_diff;
bryta;
fall RIGHT_MOTOR:
used_motor -> cur_speed = right_diff;
bryta;
fall ALPHA_MOTOR:
used_motor -> cur_speed = left_diff - right_diff;
bryta;
fall DELTA_MOTOR:
used_motor -> cur_speed = (left_diff + right_diff) / 2;
bryta;
standard:
bryta;
}
}

/********************************/
/ * POSITION UPPSKATTNING FUNKTION * /
/********************************/

/ * Beräkna position roboten * /
void get_Odometers(void)
{
lång left_wheel = 0;
lång right_wheel = 0;

Double left_mm = 0.0;
Double right_mm = 0.0;

dubbla avståndet = 0,0;

left_wheel = left_cnt;
right_wheel = right_cnt;

left_diff = last_left - left_wheel;
right_diff = last_right - right_wheel;

last_left = left_wheel;
last_right = right_wheel;

left_mm = ((dubbel) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((dubbel) right_diff) / TICK_PER_MM_RIGHT;

avståndet = (left_mm + right_mm) / 2;
total_distance += avstånd;
bot_command_delta.current_distance += avstånd;

Maximus.theta += (right_mm - left_mm) / DIAMETER.
bot_command_alpha.current_distance += (right_mm - left_mm) / DIAMETER.

om (maximus.theta > PI)
Maximus.theta-= TWOPI;
om (maximus.theta < (-PI))
Maximus.theta += TWOPI;

Maximus.pos_Y += avstånd * sin(maximus.theta);
Maximus.pos_X += avstånd * cos(maximus.theta);

update_motor(&left_motor);
update_motor(&right_motor);
update_motor(&alpha_motor);
update_motor(&delta_motor);

}

Så, låt oss se vad den gör (det är inte riktigt begripligt vid första ögonkastet):
-init_motors(void)
det inits alla variabler av motorerna som PID konstanterna, acceleration och max hastigheten vi vill

-do_motion_control(void)
Denna funktion är mycket viktigt. Det beräkna vinkel och avstånd värde med funktionen PID och lägga den i den motsvarande motorn

-set_new_command (struct RobotCommand * cmd, långa avstånd)
Ge ett nytt kommando till en motor, till exempel för avstånd motorn (delta) kan vi säga honom för att gå en 100millimeters

-långa compute_position_PID (struct RobotCommand * cmd, struct motor * used_motor)
Den här funktionen Beräkna PID för en motor med hans konstant, acceleration och max hastighet

-goto_xy (double x, dubbel y) och goto_xy_back (double x, dubbel y)
Dessa funktioner används för att ge en vägpunkt till roboten

Du kan nu göra något som det (att ge waypointen till din robot och se honom åka dit ensam):