Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

main.cpp@19:30942f018252, 2016-01-07 (annotated)

Committer:
IceTeam
Date:
Thu Jan 07 18:22:38 2016 +0000
Revision:
19:30942f018252
Parent:
18:0f1fefe78266 
Une version toute pourrie, pleine de bug. Toute les verification d'accessibilit? avec getHeight ont ete mise en commentaire, il faudra les decommenter;

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