De raming van de positie van een wielen bot met arduino. (4 / 5 stap)

Stap 4: Let's go to dit standpunt u weinig bot!

Dus, heb je een robot die weet waar hij is, maar je moet controle van alle zijn bewegingen. Niet erg goed!
Laten we repareren door het beheersen van de robot alleen met waypoints, die is veel leuker.

Om dat te doen, hoeft u niet elke andere mechanica noch elektronica! Het is pure software ;)

Laten we de code bekijken (het is een beetje lang en niet uitgebreid op het eerste gezicht:

/*
* ----------------------------------------------------------------------------
* "De bier-WARE LICENSE" (revisie 42):
* JBot schreef dit bestand. Zolang u dit behouden merk je
* kan doen wat je wilt met dit spul. Als we een dag ontmoeten, en u denkt
* Dit spul is de moeite waard, u kunt kopen me een biertje in ruil.
* ----------------------------------------------------------------------------
*/

Andere omvat
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >

/***********/
/ * Definieert * /
/***********/
#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 / / afstand tussen de 2 wielen

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radialen naar graden conversie * /

Soorten motor
#define ALPHA_MOTOR 0
#define DELTA_MOTOR 1
#define LEFT_MOTOR 2
#define RIGHT_MOTOR 3

#define ALPHADELTA 0
#define LEFTRIGHT 1

#define COMMAND_DONE 0
#define PROCESSING_COMMAND 1
#define WAITING_BEGIN 2
#define fout 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

/***********************/
/ * Specifieke structuren * /
/***********************/
struct motor {}
type int;
ondertekende lange des_speed;
ondertekende lange cur_speed;
lange last_error;
lange error_sum;
int kP;
int kI;
int kD;
ondertekende lange accel;
ondertekend lang bleef;
ondertekende lange max_speed;
dubbele afstand;
};

struct robot {}
dubbele pos_X;
dubbele pos_Y;
dubbele theta;
float yaw;
float toonhoogte;
float roll;
float yaw_offset;
};

struct RobotCommand {}
char staat;
dubbele current_distance;
dubbele desired_distance;
};

struct {punt}
int x;
int y;
};

/********************/
/ * Globale variabelen * /
/********************/
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;

vluchtige lange left_cnt = 0;
vluchtige lange right_cnt = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

dubbele total_distance = 0,0;

/***********************/
/ * INTERRUPT FUNCTIES * /
/***********************/

Externe 4 Interrupt service routine = > PIN2
ISR(INT4_vect)
{
Als ((PINB & 0x10)! = 0) {}
Als ((PINE & 0x10)! = 0)
left_cnt--;
anders
left_cnt ++;
} else {}
Als ((PINE & 0x10) == 0)
left_cnt--;
anders
left_cnt ++;
}

}

Externe 5 Interrupt service routine = > PIN3
ISR(INT5_vect)
{
Als ((PINK & 0x80)! = 0) {}
Als ((PINE & 0x20)! = 0)
right_cnt ++;
anders
right_cnt--;
} else {}
Als ((PINE & 0x20) == 0)
right_cnt ++;
anders
right_cnt--;
}

}

PIN verandering 0-7 interrupt service routine = > PIN10
ISR(PCINT0_vect)
{
Als ((PINE & 0x10)! = 0) {}
Als ((PINB & 0x10)! = 0) {}
left_cnt ++;
} else
left_cnt--;
} else {}
Als ((PINB & 0x10) == 0) {}
left_cnt ++;
} else
left_cnt--;
}

}

PIN wijzigen 16-23 interrupt service routine = > PIN-ADC15
ISR(PCINT2_vect)
{
Als ((PINE & 0x20)! = 0) {}
Als ((PINK & 0x80)! = 0)
right_cnt--;
anders
right_cnt ++;
} else {}
Als ((PINK & 0x80) == 0)
right_cnt--;
anders
right_cnt ++;
}

}

Timer 1 overflow interrupt service routine
ISR(TIMER1_OVF_vect)
{
Sei(); inschakelen interrupts
get_Odometers();

do_motion_control();
move_motors(ALPHADELTA); Update de motorsnelheid

}

/*************************/
/ * INITIALISATIE VAN HET SYSTEEM * /
/*************************/
VOID Setup
{

Initialisatie van de Input/Output-poorten
Poort een initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTA = 0X00;
DDRA = 0X00;

Poort B initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = uit
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTB = 0X00;
DDRB = 0X00;

Poort C initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTC = 0X00;
DDRC = 0X00;

Poort D initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTD = 0X00;
DDRD = 0X00;

Port E initialisatie
Func2 = In Func1 In Func0 = = In
State2 = T gebracht1 = State0 voor T = T
PORTE = 0X00;
DDRE = 0X00;

PORTK = 0X00;
DDRK = 0X00;

pinMode (13, OUTPUT);

Timer/teller 1 initialisatie
Klokbron: systeemklok
Klok waarde: 62,500 kHz
Modus: Ph. juiste PWM top = 00FFh
OC1A output: Discon.
OC1B output: Discon.
OC1C output: Discon.
Lawaai van de Canceler: uit
Input Capture aan dalende rand
Timer 1 Overflow Interrupt: op
Input Capture Interrupt: uit
Vergelijk een Match-Interrupt: uit
Vergelijk B Match Interrupt: uit
Vergelijk C Match Interrupt: uit
TCCR1A = 0X01;
TCCR1B = 0X04;
TCNT1H = 0X00;
TCNT1L = 0X00;
ICR1H = 0X00;
ICR1L = 0X00;
OCR1AH = 0X00;
OCR1AL = 0X00;
OCR1BH = 0X00;
OCR1BL = 0X00;
OCR1CH = 0X00;
OCR1CL = 0X00;

Externe Interrupt(s) initialisatie
EICRA = 0X00;
EICRB = 0X05;
EIMSK = 0X30;
EIFR = 0X30;
Onderbreken op PCINT
PCICR = 0X05;
PCIFR = 0X05;
PCMSK0 = 0X10;
PCMSK1 = 0X00;
PCMSK2 = 0X80;

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

/******************************/
/ * Initialisatie van de code * /
/******************************/
init_motors(); Init motoren
init_Robot(&Maximus); Init robot status

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

Global inschakelen interrupts
Sei();

delay(10);

}

/******************/
/ * MAIN CODE LUS * /
/******************/
void loop
{
Plaats hier uw code

vertraging(20);

}

/****************************/
/ * INITIALISATIE FUNCTIES * /
/****************************/
VOID init_Robot (struct robot * my_robot)
{
my_robot -> pos_X = 0;
my_robot -> pos_Y = 0;
my_robot -> theta = 0;
my_robot -> yaw = 0,0;
my_robot -> pitch = 0,0;
my_robot -> roll = 0,0;
my_robot -> yaw_offset = 0,0;
}

VOID init_Command (struct RobotCommand * cmd)
{
cmd -> staat = COMMAND_DONE;
cmd -> current_distance = 0;
cmd -> desired_distance = 0;
}

VOID init_motors(void)
{
/ * Links motor initialiseren * /
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;

/ * Rechts motor initialiseren * /
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 initialiseren * /
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 initialiseren * /
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;
}

/*******************************/
/ * FUNCTIE VAN DE CONTROLE VAN DE BEWEGING * /
/*******************************/
VOID do_motion_control(void)
{

PID hoek
alpha_motor.des_speed = compute_position_PID (bot_command_alpha, & alpha_motor);

PID afstand
Als ((bot_command_alpha.state == WAITING_BEGIN) || (bot_command_alpha.State == PROCESSING_COMMAND)) {/ / Als alfa motor nog niet klaar voor het verkeer

} else {}
Als ((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, lange afstand)
{
cmd -> staat = WAITING_BEGIN;
cmd -> current_distance = 0;
cmd -> desired_distance = afstand;
}

lange compute_position_PID (struct RobotCommand * cmd, struct motor * used_motor)
{
lange P, I, D;
lang errDif, dwalen;
lange tmp = 0;

Als (cmd -> staat == WAITING_BEGIN) {}
cmd -> staat = PROCESSING_COMMAND;
}

Als (used_motor -> Typ == ALPHA_MOTOR)
Err = cmd -> desired_distance * 10 - cmd -> current_distance * 10 * RAD2DEG;
anders
Err = cmd -> desired_distance - cmd -> current_distance;

used_motor -> error_sum += dwalen; Som van de fout
Als (used_motor -> error_sum > 10)
used_motor -> error_sum = 10;
Als (used_motor -> error_sum < -10)
used_motor -> error_sum = -10;

errDif = zich vergissen - used_motor -> last_error; Berekenen van de variatie van de fout

used_motor -> last_error = dwalen;

P = err * used_motor -> kP; Proportionnal
Ik = used_motor -> error_sum * used_motor -> kI; Integraal
D = errDif * used_motor -> kD; Afgeleide

tmp = (P + I + D);

Als (abs(tmp) < abs (used_motor -> des_speed)) {/ / vertraging
Als (tmp > (used_motor -> des_speed + used_motor -> bleef))
tmp = (used_motor -> des_speed + used_motor -> bleef);
anders als (tmp < (used_motor -> des_speed - used_motor -> bleef))
tmp = (used_motor -> des_speed - used_motor -> bleef);
} else {/ / versnelling
Als (tmp > (used_motor -> des_speed + used_motor -> accel))
tmp = (used_motor -> des_speed + used_motor -> accel);
anders als (tmp < (used_motor -> des_speed - used_motor -> accel))
tmp = (used_motor -> des_speed - used_motor -> accel);
}

Als (tmp > (used_motor -> max_speed))
tmp = (used_motor -> max_speed);
Als (tmp <-(used_motor -> max_speed))
tmp =-(used_motor -> max_speed);

Als (used_motor -> Typ == ALPHA_MOTOR) {}
Als ((cmd -> staat == PROCESSING_COMMAND) & & (abs(err) < 3)
& & (abs(errDif) < 3)) {/ / 2 voordat
cmd -> staat = COMMAND_DONE;
}
} else {}
Als ((cmd -> staat == PROCESSING_COMMAND) & & (abs(err) < 6)
& & (abs(errDif) < 5)) {/ / 2 voordat
cmd -> staat = COMMAND_DONE;
}
}

retourneren van tmp;
}

Berekenen van de afstand tot het doen ga naar (x, y)
dubbele distance_coord (struct robot * my_robot, dubbel x1, dubbele y1)
{
dubbele x = 0;
x = sqrt (pow (fabs (x1-my_robot -> pos_X), 2) + pow (fabs (y1 - my_robot -> pos_Y), 2));
return x;
}

Bereken de hoek om te doen ga naar (x, y)
dubbele angle_coord (struct robot * my_robot, dubbel x1, dubbele y1)
{
dubbele angletodo = 0;
Als ((x1 < my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {}
angletodo = van -PI / 2 - atan (fabs ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else if ((x1 > my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {}
angletodo = - atan (fabs ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else if ((x1 > my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {}
angletodo = atan (fabs ((y1 - my_robot -> pos_Y) / (x1-my_robot -> pos_X)));
} else if ((x1 < my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {}
angletodo = PI / 2 + atan (fabs ((x1-my_robot -> pos_X) / (y1 - my_robot -> pos_Y)));
} else if ((x1 < my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = -PI;
} else if ((x1 > my_robot -> pos_X) & & (y1 == my_robot -> pos_Y)) {/ /
angletodo = 0;
} else if ((x1 == my_robot -> pos_X) & & (y1 < my_robot-> pos_Y)) {/ /
angletodo = van -PI / 2;
} else if ((x1 == my_robot -> pos_X) & & (y1 > my_robot -> pos_Y)) {/ /
angletodo = PI / 2;
} else
angletodo = 0;

angletodo = angletodo - my_robot -> theta;

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

Return angletodo;
}

VOID goto_xy (dubbele x, dubbele y)
{
dubbele 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 (dubbele x, dubbele y)
{
dubbele ang, dist;

ang = angle_coord (& maximus, x, y);
Als (ang < 0)
ang = (ang + PI) * RAD2DEG;
anders
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;
}

/********************/
/ * MOTOREN FUNCTIES * /
/********************/
VOID move_motors(char type)
{
Als (type == ALPHADELTA) {}
write_RoboClaw_speed_M1M2 (128, delta_motor.des_speed - alpha_motor.des_speed, delta_motor.des_speed + alpha_motor.des_speed);
ZET UW DRIVE MOTOR CODE HIER
}
else {}
write_RoboClaw_speed_M1M2 (128, left_motor.des_speed, right_motor.des_speed);
ZET UW DRIVE MOTOR CODE HIER
}
}

VOID update_motor (struct motor * used_motor)
{
schakelen (used_motor -> Typ) {}
Case LEFT_MOTOR:
used_motor -> cur_speed = left_diff;
breken;
Case RIGHT_MOTOR:
used_motor -> cur_speed = right_diff;
breken;
Case ALPHA_MOTOR:
used_motor -> cur_speed = left_diff - right_diff;
breken;
Case DELTA_MOTOR:
used_motor -> cur_speed = (left_diff + right_diff) / 2;
breken;
standaard:
breken;
}
}

/********************************/
/ * POSITIE SCHATTING FUNCTIE * /
/********************************/

/ * Het berekenen van de positie van de robot * /
VOID get_Odometers(void)
{
lange left_wheel = 0;
lange right_wheel = 0;

dubbele left_mm = 0,0;
dubbele right_mm = 0,0;

dubbele afstand = 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 = ((dubbele) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((dubbele) right_diff) / TICK_PER_MM_RIGHT;

afstand = (left_mm + right_mm) / 2;
total_distance += afstand;
bot_command_delta.current_distance += afstand;

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

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

Maximus.pos_Y += afstand * sin(maximus.theta);
Maximus.pos_X += afstand * cos(maximus.theta);

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

}

Dus, laten we eens kijken wat het doet (het is niet echt te begrijpen op het eerste gezicht):
-init_motors(void)
het inits alle variabelen van de motoren zoals de PID-constanten zijn, de versnelling en de max snelheid die we willen

-do_motion_control(void)
Deze functie is erg belangrijk. Het berekenen van de hoek en de waarde van de afstand met de PID-functie en zet het in de overeenkomstige motor

-set_new_command (struct RobotCommand * cmd, lange afstand)
Een nieuwe opdracht geven een motor, voor bijvoorbeeld de afstand motor (delta) kunnen we zeggen hem om te gaan van een 100millimeters

-lange compute_position_PID (struct RobotCommand * cmd, struct motor * used_motor)
Deze functie berekent de PID voor een motor met behulp van zijn constante, versnelling en max snelheid

-goto_xy (double x, dubbele y) en goto_xy_back (dubbele x, dubbele y)
Deze functies worden gebruikt om een waypoint aan de robot

U kunt nu iets doen als dat (waypoint om uw robot en zien hem gaan er alleen):