ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

main.cpp.orig@17:e32b4b54fc04, 2016-01-07 (annotated)

Committer:
IceTeam
Date:
Thu Jan 07 14:08:33 2016 +0100
Revision:
17:e32b4b54fc04
Child:
26:6c5c453602ff
Test AStar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 17:e32b4b54fc04 1 #include "Odometry.h"
IceTeam 17:e32b4b54fc04 2
IceTeam 17:e32b4b54fc04 3 #define dt 10000
IceTeam 17:e32b4b54fc04 4 #define ATTENTE 0
IceTeam 17:e32b4b54fc04 5 #define GO 1
IceTeam 17:e32b4b54fc04 6 #define STOP 2
IceTeam 17:e32b4b54fc04 7
IceTeam 17:e32b4b54fc04 8 InterruptIn mybutton(USER_BUTTON);
IceTeam 17:e32b4b54fc04 9 DigitalIn button(USER_BUTTON);
IceTeam 17:e32b4b54fc04 10 DigitalOut led(LED1);
IceTeam 17:e32b4b54fc04 11 Ticker ticker;
IceTeam 17:e32b4b54fc04 12 //Serial pc(USBTX, USBRX);
IceTeam 17:e32b4b54fc04 13 Serial pc(PA_9, PA_10);
IceTeam 17:e32b4b54fc04 14 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 17:e32b4b54fc04 15 Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
IceTeam 17:e32b4b54fc04 16
IceTeam 17:e32b4b54fc04 17 int i = 0;
IceTeam 17:e32b4b54fc04 18
IceTeam 17:e32b4b54fc04 19 void update_main(void);
IceTeam 17:e32b4b54fc04 20 void init(void);
IceTeam 17:e32b4b54fc04 21 void pressed(void);
IceTeam 17:e32b4b54fc04 22 void unpressed(void);
IceTeam 17:e32b4b54fc04 23
IceTeam 17:e32b4b54fc04 24 /** Debut du programme */
IceTeam 17:e32b4b54fc04 25 int main(void)
IceTeam 17:e32b4b54fc04 26 {
IceTeam 17:e32b4b54fc04 27 init();
IceTeam 17:e32b4b54fc04 28 //roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, -116878, accel_angle, vitesse_angle, deccel_angle, 116878, 1);
IceTeam 17:e32b4b54fc04 29 odo.GotoXY(1800,1500);
IceTeam 17:e32b4b54fc04 30 odo.GotoXY(2500,500);
IceTeam 17:e32b4b54fc04 31 odo.GotoXY(2000,300);
IceTeam 17:e32b4b54fc04 32 odo.GotoXYT(300,1000,0);
IceTeam 17:e32b4b54fc04 33 //for(int n=0; n<40; n++) odo.setTheta();
IceTeam 17:e32b4b54fc04 34 odo.GotoThet(-PI);
IceTeam 17:e32b4b54fc04 35 odo.GotoThet(0);
IceTeam 17:e32b4b54fc04 36 wait(2000);
IceTeam 17:e32b4b54fc04 37 while(1);
IceTeam 17:e32b4b54fc04 38 }
IceTeam 17:e32b4b54fc04 39
IceTeam 17:e32b4b54fc04 40 void init(void)
IceTeam 17:e32b4b54fc04 41 {
IceTeam 17:e32b4b54fc04 42 pc.baud(9600);
IceTeam 17:e32b4b54fc04 43 pc.printf("Hello from main !\n\r");
IceTeam 17:e32b4b54fc04 44 wait_ms(500);
IceTeam 17:e32b4b54fc04 45
IceTeam 17:e32b4b54fc04 46 odo.setPos(300, 1000, 0);
IceTeam 17:e32b4b54fc04 47 roboclaw.ForwardM1(ADR, 0);
IceTeam 17:e32b4b54fc04 48 roboclaw.ForwardM2(ADR, 0);
IceTeam 17:e32b4b54fc04 49
IceTeam 17:e32b4b54fc04 50 while(button);
IceTeam 17:e32b4b54fc04 51 wait(1);
IceTeam 17:e32b4b54fc04 52 mybutton.fall(&pressed);
IceTeam 17:e32b4b54fc04 53 mybutton.rise(&unpressed);
IceTeam 17:e32b4b54fc04 54 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 17:e32b4b54fc04 55 }
IceTeam 17:e32b4b54fc04 56
IceTeam 17:e32b4b54fc04 57 void update_main(void)
IceTeam 17:e32b4b54fc04 58 {
IceTeam 17:e32b4b54fc04 59 odo.update_odo();
IceTeam 17:e32b4b54fc04 60 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
IceTeam 17:e32b4b54fc04 61 //pc.printf("Theta : %3.2f\n\r", odo.getTheta()*180/PI);
IceTeam 17:e32b4b54fc04 62 pc.printf("X : %4.2f\n\r", odo.getX());
IceTeam 17:e32b4b54fc04 63 //if(pc.readable()) if(pc.getc()=='l') led = !led;
IceTeam 17:e32b4b54fc04 64 }
IceTeam 17:e32b4b54fc04 65
IceTeam 17:e32b4b54fc04 66 void pressed(void)
IceTeam 17:e32b4b54fc04 67 {
IceTeam 17:e32b4b54fc04 68 if(i==0) {
IceTeam 17:e32b4b54fc04 69 roboclaw.ForwardM1(ADR, 0);
IceTeam 17:e32b4b54fc04 70 roboclaw.ForwardM2(ADR, 0);
IceTeam 17:e32b4b54fc04 71 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
IceTeam 17:e32b4b54fc04 72 i++;
IceTeam 17:e32b4b54fc04 73 }
IceTeam 17:e32b4b54fc04 74 }
IceTeam 17:e32b4b54fc04 75
IceTeam 17:e32b4b54fc04 76 void unpressed(void)
IceTeam 17:e32b4b54fc04 77 {
IceTeam 17:e32b4b54fc04 78 if(i==1) {
IceTeam 17:e32b4b54fc04 79 i--;
IceTeam 17:e32b4b54fc04 80 }
IceTeam 17:e32b4b54fc04 81 }