Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

main.cpp@40:ca4dd3faffa8, 2016-04-13 (annotated)

Committer:
IceTeam
Date:
Wed Apr 13 11:27:01 2016 +0000
Revision:
40:ca4dd3faffa8
Parent:
38:da3a2c781672 
First publish;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 31:8bcc3a0bfa8a 1 #include "Odometry/Odometry.h"
IceTeam 40:ca4dd3faffa8 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();
IceTeam 40:ca4dd3faffa8 30 double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 31 double distance_v = 200.0;
IceTeam 40:ca4dd3faffa8 32 float the;
sype 38:da3a2c781672 33
IceTeam 40:ca4dd3faffa8 34 odo.setPos(0, 1000, 0);
IceTeam 40:ca4dd3faffa8 35 point dep (odo.getX(), odo.getY());
IceTeam 40:ca4dd3faffa8 36 point arr (1400, 1000);
IceTeam 28:142ebc5410f3 37
IceTeam 40:ca4dd3faffa8 38 map m;
IceTeam 40:ca4dd3faffa8 39 m.addObs(obsCarr (550, 1030, 250, 230));
IceTeam 40:ca4dd3faffa8 40 m.addObs(obsCarr (740, 720, 240, 240));
IceTeam 40:ca4dd3faffa8 41 m.addObs(obsCarr (1170, 1260, 220, 220));
IceTeam 30:58bfac39e701 42
IceTeam 40:ca4dd3faffa8 43 /*m.addObst (carre (1000, 1000, 210, 210));
IceTeam 40:ca4dd3faffa8 44 m.addObst (carre (1300, 700, 180, 180));*/
IceTeam 40:ca4dd3faffa8 45 m.FindWay (dep, arr);
IceTeam 40:ca4dd3faffa8 46 logger.printf("On a cherche un chemin\n\r");
IceTeam 13:a394e27b8cb2 47
IceTeam 40:ca4dd3faffa8 48 if (m.getEnded()) {
IceTeam 40:ca4dd3faffa8 49 nVector<pointParcours> p = m.getParc ();
IceTeam 40:ca4dd3faffa8 50 logger.printf ("Il y a %d points de parcours\n\r", p.size());
IceTeam 40:ca4dd3faffa8 51 for (int i = 0; i < p.size (); i++) {
IceTeam 40:ca4dd3faffa8 52 logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
IceTeam 40:ca4dd3faffa8 53 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 40:ca4dd3faffa8 54 odo.GotoXY(p[i].getX(), p[i].getY());
IceTeam 40:ca4dd3faffa8 55 }
IceTeam 18:0f1fefe78266 56 }
IceTeam 40:ca4dd3faffa8 57 else {
IceTeam 40:ca4dd3faffa8 58 logger.printf("Chemin pas trouve ...");
IceTeam 27:ae68960316f1 59 }
IceTeam 30:58bfac39e701 60
IceTeam 36:2d7357a385bc 61 //odo.GotoThet(PI);
IceTeam 36:2d7357a385bc 62 odo.GotoThet(0);
IceTeam 36:2d7357a385bc 63 //odo.TestEntraxe(3);
IceTeam 31:8bcc3a0bfa8a 64
sype 10:ae3178aa94e9 65 //odo.GotoThet(-PI/2);
sype 10:ae3178aa94e9 66 wait(2000);
IceTeam 40:ca4dd3faffa8 67 while (1);
sype 38:da3a2c781672 68 //odo.GotoXYT(2250,500,0);*/
IceTeam 40:ca4dd3faffa8 69 /*roboclaw.ForwardM1(ADR, 0);
IceTeam 40:ca4dd3faffa8 70 roboclaw.ForwardM2(ADR, 0);
IceTeam 40:ca4dd3faffa8 71 int state = 0;
sype 38:da3a2c781672 72 while(1)
sype 38:da3a2c781672 73 {
IceTeam 40:ca4dd3faffa8 74 // while(logger.readable())
IceTeam 40:ca4dd3faffa8 75 //{
sype 38:da3a2c781672 76 char c = logger.getc();
sype 38:da3a2c781672 77 if(c=='z')
sype 38:da3a2c781672 78 {
IceTeam 40:ca4dd3faffa8 79 if (state != 1) {
IceTeam 40:ca4dd3faffa8 80 state = 1;
IceTeam 40:ca4dd3faffa8 81 logger.printf("Z\r\n");
IceTeam 40:ca4dd3faffa8 82 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
IceTeam 40:ca4dd3faffa8 83 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
IceTeam 40:ca4dd3faffa8 84 }
sype 38:da3a2c781672 85 }
sype 38:da3a2c781672 86 else if(c == 's')
sype 38:da3a2c781672 87 {
IceTeam 40:ca4dd3faffa8 88 if (state != 2) {
IceTeam 40:ca4dd3faffa8 89 state = 2;
IceTeam 40:ca4dd3faffa8 90 roboclaw.ForwardM1(ADR, 0);
IceTeam 40:ca4dd3faffa8 91 roboclaw.ForwardM2(ADR, 0);
IceTeam 40:ca4dd3faffa8 92 }
sype 38:da3a2c781672 93 }
sype 38:da3a2c781672 94 else if(c == 'd')
sype 38:da3a2c781672 95 {
IceTeam 40:ca4dd3faffa8 96 if (state != 3) {
IceTeam 40:ca4dd3faffa8 97 state = 3;
IceTeam 40:ca4dd3faffa8 98 logger.printf("D\r\n");
IceTeam 40:ca4dd3faffa8 99 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
IceTeam 40:ca4dd3faffa8 100 roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
IceTeam 40:ca4dd3faffa8 101 }
sype 38:da3a2c781672 102 }
sype 38:da3a2c781672 103 else if(c == 'q')
sype 38:da3a2c781672 104 {
IceTeam 40:ca4dd3faffa8 105 if (state != 4) {
IceTeam 40:ca4dd3faffa8 106 state = 4;
IceTeam 40:ca4dd3faffa8 107 logger.printf("Q\r\n");
IceTeam 40:ca4dd3faffa8 108 roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
IceTeam 40:ca4dd3faffa8 109 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
IceTeam 40:ca4dd3faffa8 110 }
sype 38:da3a2c781672 111 }
sype 38:da3a2c781672 112 else if(c == 'x')
sype 38:da3a2c781672 113 {
IceTeam 40:ca4dd3faffa8 114 if (state != 5) {
IceTeam 40:ca4dd3faffa8 115 state = 5;
IceTeam 40:ca4dd3faffa8 116 roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
IceTeam 40:ca4dd3faffa8 117 }
sype 38:da3a2c781672 118 }
IceTeam 40:ca4dd3faffa8 119 else if (c == '\0') { ; }
IceTeam 40:ca4dd3faffa8 120 else {
IceTeam 40:ca4dd3faffa8 121 if (state != 0) {
IceTeam 40:ca4dd3faffa8 122 roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
IceTeam 40:ca4dd3faffa8 123 state = 0;
IceTeam 40:ca4dd3faffa8 124 }
IceTeam 40:ca4dd3faffa8 125 }
IceTeam 40:ca4dd3faffa8 126 // }
IceTeam 40:ca4dd3faffa8 127 // roboclaw.ForwardM1(ADR, 0);
IceTeam 40:ca4dd3faffa8 128 // roboclaw.ForwardM2(ADR, 0);
IceTeam 40:ca4dd3faffa8 129 }*/
sype 2:abdf8c6823a1 130 }
sype 2:abdf8c6823a1 131
sype 2:abdf8c6823a1 132 void init(void)
sype 0:ad9600df4a70 133 {
sype 38:da3a2c781672 134 roboclaw.ForwardM1(ADR, 0);
sype 38:da3a2c781672 135 roboclaw.ForwardM2(ADR, 0);
sype 38:da3a2c781672 136 logger.baud(9600);
sype 38:da3a2c781672 137 logger.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 138 wait_ms(500);
sype 10:ae3178aa94e9 139
IceTeam 27:ae68960316f1 140 odo.setPos(0, 1000, 0);
sype 38:da3a2c781672 141
sype 10:ae3178aa94e9 142 while(button);
sype 10:ae3178aa94e9 143 wait(1);
sype 10:ae3178aa94e9 144 mybutton.fall(&pressed);
IceTeam 40:ca4dd3faffa8 145 mybutton.rise(&unpressed);
sype 0:ad9600df4a70 146 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 40:ca4dd3faffa8 147
IceTeam 40:ca4dd3faffa8 148 logger.printf("End init\n\r");
sype 0:ad9600df4a70 149 }
sype 0:ad9600df4a70 150
sype 0:ad9600df4a70 151 void update_main(void)
sype 0:ad9600df4a70 152 {
sype 0:ad9600df4a70 153 odo.update_odo();
sype 10:ae3178aa94e9 154 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 2:abdf8c6823a1 155 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 156 }
sype 10:ae3178aa94e9 157
sype 10:ae3178aa94e9 158 void pressed(void)
sype 10:ae3178aa94e9 159 {
sype 10:ae3178aa94e9 160 if(i==0) {
sype 10:ae3178aa94e9 161 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 162 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 163 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 164 i++;
sype 10:ae3178aa94e9 165 }
sype 10:ae3178aa94e9 166 }
sype 10:ae3178aa94e9 167
sype 10:ae3178aa94e9 168 void unpressed(void)
sype 10:ae3178aa94e9 169 {
sype 10:ae3178aa94e9 170 if(i==1) {
sype 10:ae3178aa94e9 171 i--;
sype 10:ae3178aa94e9 172 }
sype 10:ae3178aa94e9 173 }