ARES / Mbed 2 deprecated TestMyPathFind

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

main.cpp@37:a17f8a61bc0d, 2016-02-06 (annotated)

Committer:
IceTeam
Date:
Sat Feb 06 10:13:13 2016 +0000
Revision:
37:a17f8a61bc0d
Parent:
36:2d7357a385bc 
Save pre commande;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 31:8bcc3a0bfa8a 1 #include "Odometry/Odometry.h"
IceTeam 12:5355aed288b0 2 #include "Map/Map.h"
sype 0:ad9600df4a70 3
sype 0:ad9600df4a70 4 #define dt 10000
sype 10:ae3178aa94e9 5 #define ATTENTE 0
sype 10:ae3178aa94e9 6 #define GO 1
sype 10:ae3178aa94e9 7 #define STOP 2
IceTeam 37:a17f8a61bc0d 8 #define ENTRAXE 247.5
sype 0:ad9600df4a70 9
sype 10:ae3178aa94e9 10 InterruptIn mybutton(USER_BUTTON);
sype 10:ae3178aa94e9 11 DigitalIn button(USER_BUTTON);
sype 2:abdf8c6823a1 12 DigitalOut led(LED1);
sype 0:ad9600df4a70 13 Ticker ticker;
sype 10:ae3178aa94e9 14 //Serial pc(USBTX, USBRX);
sype 10:ae3178aa94e9 15 Serial pc(PA_9, PA_10);
IceTeam 21:077bc7634b90 16 Serial logger (PA_9, PA_10);
sype 10:ae3178aa94e9 17 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 31:8bcc3a0bfa8a 18 /* Changement entraxe : 252->253 */
IceTeam 36:2d7357a385bc 19 Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
sype 10:ae3178aa94e9 20
sype 10:ae3178aa94e9 21 int i = 0;
sype 0:ad9600df4a70 22
sype 0:ad9600df4a70 23 void update_main(void);
sype 2:abdf8c6823a1 24 void init(void);
sype 10:ae3178aa94e9 25 void pressed(void);
sype 10:ae3178aa94e9 26 void unpressed(void);
sype 0:ad9600df4a70 27
IceTeam 9:e39b218ab20d 28 /** Debut du programme */
sype 0:ad9600df4a70 29 int main(void)
sype 10:ae3178aa94e9 30 {
sype 10:ae3178aa94e9 31 double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 32 double distance_v = 200.0;
IceTeam 12:5355aed288b0 33 std::vector<SimplePoint> path;
IceTeam 12:5355aed288b0 34 Map map;
IceTeam 12:5355aed288b0 35
sype 2:abdf8c6823a1 36 init();
IceTeam 18:0f1fefe78266 37 //Construction des obstacles
IceTeam 36:2d7357a385bc 38 map.build();
IceTeam 28:142ebc5410f3 39
IceTeam 36:2d7357a385bc 40 float x=odo.getX();
IceTeam 30:58bfac39e701 41 float y=odo.getY();
IceTeam 30:58bfac39e701 42 float the = 0;
IceTeam 30:58bfac39e701 43
IceTeam 30:58bfac39e701 44 map.AStar(x, y, 1400, 1000, 75);
IceTeam 13:a394e27b8cb2 45 path = map.path;
IceTeam 13:a394e27b8cb2 46
IceTeam 18:0f1fefe78266 47 for(int i=0; i<path.size();i++) {
IceTeam 30:58bfac39e701 48 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 49 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 18:0f1fefe78266 50 }
IceTeam 27:ae68960316f1 51
IceTeam 30:58bfac39e701 52 map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
IceTeam 27:ae68960316f1 53 path = map.path;
IceTeam 27:ae68960316f1 54
IceTeam 27:ae68960316f1 55 for(int i=0; i<path.size();i++) {
IceTeam 30:58bfac39e701 56 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 57 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 27:ae68960316f1 58 }
IceTeam 30:58bfac39e701 59
IceTeam 36:2d7357a385bc 60 //odo.GotoThet(PI);
IceTeam 36:2d7357a385bc 61 odo.GotoThet(0);
IceTeam 36:2d7357a385bc 62 //odo.TestEntraxe(3);
IceTeam 36:2d7357a385bc 63 /*
IceTeam 36:2d7357a385bc 64 odo.GotoXYT(1000, 1000, 0);
IceTeam 36:2d7357a385bc 65 odo.GotoXYT(0, 1000, PI);
IceTeam 31:8bcc3a0bfa8a 66 odo.GotoThet(0);*/
IceTeam 31:8bcc3a0bfa8a 67
sype 10:ae3178aa94e9 68 //odo.GotoThet(-PI/2);
sype 10:ae3178aa94e9 69 wait(2000);
sype 10:ae3178aa94e9 70 //odo.GotoXYT(2250,500,0);
sype 2:abdf8c6823a1 71 while(1);
sype 2:abdf8c6823a1 72 }
sype 2:abdf8c6823a1 73
sype 2:abdf8c6823a1 74 void init(void)
sype 0:ad9600df4a70 75 {
sype 10:ae3178aa94e9 76 pc.baud(9600);
sype 2:abdf8c6823a1 77 pc.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 78 wait_ms(500);
sype 10:ae3178aa94e9 79
IceTeam 27:ae68960316f1 80 odo.setPos(0, 1000, 0);
sype 2:abdf8c6823a1 81 roboclaw.ForwardM1(ADR, 0);
sype 2:abdf8c6823a1 82 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 83
sype 10:ae3178aa94e9 84 while(button);
sype 10:ae3178aa94e9 85 wait(1);
sype 10:ae3178aa94e9 86 mybutton.fall(&pressed);
sype 10:ae3178aa94e9 87 mybutton.rise(&unpressed);
sype 0:ad9600df4a70 88 ticker.attach_us(&update_main,dt); // 100 Hz
sype 0:ad9600df4a70 89 }
sype 0:ad9600df4a70 90
sype 0:ad9600df4a70 91 void update_main(void)
sype 0:ad9600df4a70 92 {
sype 0:ad9600df4a70 93 odo.update_odo();
sype 10:ae3178aa94e9 94 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 2:abdf8c6823a1 95 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 96 }
sype 10:ae3178aa94e9 97
sype 10:ae3178aa94e9 98 void pressed(void)
sype 10:ae3178aa94e9 99 {
sype 10:ae3178aa94e9 100 if(i==0) {
sype 10:ae3178aa94e9 101 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 102 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 103 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 104 i++;
sype 10:ae3178aa94e9 105 }
sype 10:ae3178aa94e9 106 }
sype 10:ae3178aa94e9 107
sype 10:ae3178aa94e9 108 void unpressed(void)
sype 10:ae3178aa94e9 109 {
sype 10:ae3178aa94e9 110 if(i==1) {
sype 10:ae3178aa94e9 111 i--;
sype 10:ae3178aa94e9 112 }
sype 10:ae3178aa94e9 113 }