Romain Ame / Mbed 2 deprecated Timer71pt

Fork de Timer après le match à 61 points

Dependencies:   RoboClaw mbed

Fork of Timer by ARES

main.cpp@30:58bfac39e701, 2016-01-22 (annotated)

Committer:
IceTeam
Date:
Fri Jan 22 15:46:43 2016 +0000
Revision:
30:58bfac39e701
Parent:
29:13aa1d1a1c12
Child:
31:8bcc3a0bfa8a
Test avec un theta venant de l'odometrie;

Who changed what in which revision?

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