ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

main.cpp@14:a394e27b8cb2, 2016-01-05 (annotated)

Committer:
IceTeam
Date:
Tue Jan 05 17:11:35 2016 +0000
Revision:
14:a394e27b8cb2
Parent:
13:5355aed288b0
Child:
16:a1ede21a963b
Correction ch'ti bug

Who changed what in which revision?

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