ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

main.cpp@38:da3a2c781672, 2016-02-06 (annotated)

Committer:
sype
Date:
Sat Feb 06 10:11:28 2016 +0000
Revision:
38:da3a2c781672
Parent:
36:2d7357a385bc
Child:
39:ca4dd3faffa8
Child:
40:309f38d1e49c
Command;

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 36:2d7357a385bc 8 #define ENTRAXE 248.25
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);
IceTeam 21:077bc7634b90 15 Serial logger (PA_9, PA_10);
sype 10:ae3178aa94e9 16 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 36:2d7357a385bc 17 Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
sype 10:ae3178aa94e9 18
sype 10:ae3178aa94e9 19 int i = 0;
sype 0:ad9600df4a70 20
sype 0:ad9600df4a70 21 void update_main(void);
sype 2:abdf8c6823a1 22 void init(void);
sype 10:ae3178aa94e9 23 void pressed(void);
sype 10:ae3178aa94e9 24 void unpressed(void);
sype 0:ad9600df4a70 25
IceTeam 9:e39b218ab20d 26 /** Debut du programme */
sype 0:ad9600df4a70 27 int main(void)
sype 10:ae3178aa94e9 28 {
sype 38:da3a2c781672 29 init();
sype 38:da3a2c781672 30 /*double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 31 double distance_v = 200.0;
IceTeam 12:5355aed288b0 32 std::vector<SimplePoint> path;
IceTeam 12:5355aed288b0 33 Map map;
IceTeam 12:5355aed288b0 34
sype 38:da3a2c781672 35
IceTeam 18:0f1fefe78266 36 //Construction des obstacles
IceTeam 36:2d7357a385bc 37 map.build();
IceTeam 28:142ebc5410f3 38
IceTeam 36:2d7357a385bc 39 float x=odo.getX();
IceTeam 30:58bfac39e701 40 float y=odo.getY();
IceTeam 30:58bfac39e701 41 float the = 0;
IceTeam 30:58bfac39e701 42
IceTeam 30:58bfac39e701 43 map.AStar(x, y, 1400, 1000, 75);
IceTeam 13:a394e27b8cb2 44 path = map.path;
IceTeam 13:a394e27b8cb2 45
IceTeam 18:0f1fefe78266 46 for(int i=0; i<path.size();i++) {
IceTeam 30:58bfac39e701 47 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 48 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 18:0f1fefe78266 49 }
IceTeam 27:ae68960316f1 50
IceTeam 30:58bfac39e701 51 map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
IceTeam 27:ae68960316f1 52 path = map.path;
IceTeam 27:ae68960316f1 53
IceTeam 27:ae68960316f1 54 for(int i=0; i<path.size();i++) {
IceTeam 30:58bfac39e701 55 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 56 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 27:ae68960316f1 57 }
IceTeam 30:58bfac39e701 58
IceTeam 36:2d7357a385bc 59 //odo.GotoThet(PI);
IceTeam 36:2d7357a385bc 60 odo.GotoThet(0);
IceTeam 36:2d7357a385bc 61 //odo.TestEntraxe(3);
IceTeam 31:8bcc3a0bfa8a 62
sype 10:ae3178aa94e9 63 //odo.GotoThet(-PI/2);
sype 10:ae3178aa94e9 64 wait(2000);
sype 38:da3a2c781672 65 //odo.GotoXYT(2250,500,0);*/
sype 38:da3a2c781672 66 while(1)
sype 38:da3a2c781672 67 {
sype 38:da3a2c781672 68 while(logger.readable())
sype 38:da3a2c781672 69 {
sype 38:da3a2c781672 70 char c = logger.getc();
sype 38:da3a2c781672 71 if(c=='z')
sype 38:da3a2c781672 72 {
sype 38:da3a2c781672 73 roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, speed_angle);
sype 38:da3a2c781672 74 }
sype 38:da3a2c781672 75 else if(c == 's')
sype 38:da3a2c781672 76 {
sype 38:da3a2c781672 77 roboclaw.ForwardM1(ADR, 0);
sype 38:da3a2c781672 78 roboclaw.ForwardM2(ADR, 0);
sype 38:da3a2c781672 79 }
sype 38:da3a2c781672 80 else if(c == 'd')
sype 38:da3a2c781672 81 {
sype 38:da3a2c781672 82 roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, speed_angle);
sype 38:da3a2c781672 83 }
sype 38:da3a2c781672 84 else if(c == 'q')
sype 38:da3a2c781672 85 {
sype 38:da3a2c781672 86 roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, -speed_angle);
sype 38:da3a2c781672 87 }
sype 38:da3a2c781672 88 else if(c == 'x')
sype 38:da3a2c781672 89 {
sype 38:da3a2c781672 90 roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, -speed_angle);
sype 38:da3a2c781672 91 }
sype 38:da3a2c781672 92 }
sype 38:da3a2c781672 93 roboclaw.ForwardM1(ADR, 0);
sype 38:da3a2c781672 94 roboclaw.ForwardM2(ADR, 0);
sype 38:da3a2c781672 95 }
sype 2:abdf8c6823a1 96 }
sype 2:abdf8c6823a1 97
sype 2:abdf8c6823a1 98 void init(void)
sype 0:ad9600df4a70 99 {
sype 38:da3a2c781672 100 roboclaw.ForwardM1(ADR, 0);
sype 38:da3a2c781672 101 roboclaw.ForwardM2(ADR, 0);
sype 38:da3a2c781672 102 logger.baud(9600);
sype 38:da3a2c781672 103 logger.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 104 wait_ms(500);
sype 10:ae3178aa94e9 105
IceTeam 27:ae68960316f1 106 odo.setPos(0, 1000, 0);
sype 38:da3a2c781672 107
sype 10:ae3178aa94e9 108 while(button);
sype 10:ae3178aa94e9 109 wait(1);
sype 10:ae3178aa94e9 110 mybutton.fall(&pressed);
sype 38:da3a2c781672 111 mybutton.rise(&unpr
sype 38:da3a2c781672 112 essed);
sype 0:ad9600df4a70 113 ticker.attach_us(&update_main,dt); // 100 Hz
sype 0:ad9600df4a70 114 }
sype 0:ad9600df4a70 115
sype 0:ad9600df4a70 116 void update_main(void)
sype 0:ad9600df4a70 117 {
sype 0:ad9600df4a70 118 odo.update_odo();
sype 10:ae3178aa94e9 119 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 2:abdf8c6823a1 120 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 121 }
sype 10:ae3178aa94e9 122
sype 10:ae3178aa94e9 123 void pressed(void)
sype 10:ae3178aa94e9 124 {
sype 10:ae3178aa94e9 125 if(i==0) {
sype 10:ae3178aa94e9 126 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 127 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 128 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 129 i++;
sype 10:ae3178aa94e9 130 }
sype 10:ae3178aa94e9 131 }
sype 10:ae3178aa94e9 132
sype 10:ae3178aa94e9 133 void unpressed(void)
sype 10:ae3178aa94e9 134 {
sype 10:ae3178aa94e9 135 if(i==1) {
sype 10:ae3178aa94e9 136 i--;
sype 10:ae3178aa94e9 137 }
sype 10:ae3178aa94e9 138 }