ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

main.cpp@39:309f38d1e49c, 2016-04-05 (annotated)

Committer:
IceTeam
Date:
Tue Apr 05 15:02:12 2016 +0000
Revision:
39:309f38d1e49c
Parent:
37:da3a2c781672
Child:
41:b5a2fbc20beb
Petite sauvegarde de batard;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 31:8bcc3a0bfa8a 1 #include "Odometry/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);
IceTeam 21:077bc7634b90 14 Serial logger (PA_9, PA_10);
sype 10:ae3178aa94e9 15 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 39:309f38d1e49c 16 Odometry odo(61.7, 61.8, ENTRAXE, 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 37:da3a2c781672 28 init();
IceTeam 39:309f38d1e49c 29 /* Code AStar */
IceTeam 39:309f38d1e49c 30
IceTeam 39:309f38d1e49c 31 double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 32 double distance_v = 200.0;
IceTeam 13:5355aed288b0 33 std::vector<SimplePoint> path;
IceTeam 13:5355aed288b0 34 Map map;
IceTeam 13:5355aed288b0 35
sype 37:da3a2c781672 36
IceTeam 18:0f1fefe78266 37 //Construction des obstacles
IceTeam 36:2d7357a385bc 38 map.build();
IceTeam 28:142ebc5410f3 39
IceTeam 36:2d7357a385bc 40 float x=odo.getX();
IceTeam 30:58bfac39e701 41 float y=odo.getY();
IceTeam 30:58bfac39e701 42 float the = 0;
IceTeam 30:58bfac39e701 43
IceTeam 39:309f38d1e49c 44 map.AStar(x, y, 1600, 1000, 75);
IceTeam 14:a394e27b8cb2 45 path = map.path;
IceTeam 14:a394e27b8cb2 46
IceTeam 18:0f1fefe78266 47 for(int i=0; i<path.size();i++) {
IceTeam 30:58bfac39e701 48 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 49 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 18:0f1fefe78266 50 }
IceTeam 27:ae68960316f1 51
IceTeam 30:58bfac39e701 52 map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
IceTeam 27:ae68960316f1 53 path = map.path;
IceTeam 27:ae68960316f1 54
IceTeam 27:ae68960316f1 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 27:ae68960316f1 58 }
IceTeam 30:58bfac39e701 59
IceTeam 36:2d7357a385bc 60 //odo.GotoThet(PI);
IceTeam 36:2d7357a385bc 61 odo.GotoThet(0);
IceTeam 36:2d7357a385bc 62 //odo.TestEntraxe(3);
IceTeam 31:8bcc3a0bfa8a 63
sype 10:ae3178aa94e9 64 //odo.GotoThet(-PI/2);
sype 10:ae3178aa94e9 65 wait(2000);
IceTeam 39:309f38d1e49c 66 //odo.GotoXYT(2250,500,0);
IceTeam 39:309f38d1e49c 67
IceTeam 39:309f38d1e49c 68 //odo.TestEntraxe(5);
IceTeam 39:309f38d1e49c 69 //odo.Forward(1000);
IceTeam 39:309f38d1e49c 70
IceTeam 39:309f38d1e49c 71 /* Code JPO :
IceTeam 39:309f38d1e49c 72 roboclaw.ForwardM1(ADR, 0);
IceTeam 39:309f38d1e49c 73 roboclaw.ForwardM2(ADR, 0);
IceTeam 39:309f38d1e49c 74 int state = 0;
sype 37:da3a2c781672 75 while(1)
sype 37:da3a2c781672 76 {
IceTeam 39:309f38d1e49c 77 // while(logger.readable())
IceTeam 39:309f38d1e49c 78 //{
sype 37:da3a2c781672 79 char c = logger.getc();
sype 37:da3a2c781672 80 if(c=='z')
sype 37:da3a2c781672 81 {
IceTeam 39:309f38d1e49c 82 if (state != 1) {
IceTeam 39:309f38d1e49c 83 state = 1;
IceTeam 39:309f38d1e49c 84 logger.printf("Avant (Z) \r\n");
IceTeam 39:309f38d1e49c 85 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
IceTeam 39:309f38d1e49c 86 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
IceTeam 39:309f38d1e49c 87 }
sype 37:da3a2c781672 88 }
sype 37:da3a2c781672 89 else if(c == 's')
sype 37:da3a2c781672 90 {
IceTeam 39:309f38d1e49c 91 if (state != 2) {
IceTeam 39:309f38d1e49c 92 state = 2;
IceTeam 39:309f38d1e49c 93 logger.printf("Stop (S) \r\n");
IceTeam 39:309f38d1e49c 94 roboclaw.ForwardM1(ADR, 0);
IceTeam 39:309f38d1e49c 95 roboclaw.ForwardM2(ADR, 0);
IceTeam 39:309f38d1e49c 96 }
sype 37:da3a2c781672 97 }
sype 37:da3a2c781672 98 else if(c == 'd')
sype 37:da3a2c781672 99 {
IceTeam 39:309f38d1e49c 100 if (state != 3) {
IceTeam 39:309f38d1e49c 101 state = 3;
IceTeam 39:309f38d1e49c 102 logger.printf("Droite (D) \r\n");
IceTeam 39:309f38d1e49c 103 roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
IceTeam 39:309f38d1e49c 104 roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
IceTeam 39:309f38d1e49c 105 }
sype 37:da3a2c781672 106 }
sype 37:da3a2c781672 107 else if(c == 'q')
sype 37:da3a2c781672 108 {
IceTeam 39:309f38d1e49c 109 if (state != 4) {
IceTeam 39:309f38d1e49c 110 state = 4;
IceTeam 39:309f38d1e49c 111 logger.printf("Gauche (Q)\r\n");
IceTeam 39:309f38d1e49c 112 roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
IceTeam 39:309f38d1e49c 113 roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
IceTeam 39:309f38d1e49c 114 }
sype 37:da3a2c781672 115 }
sype 37:da3a2c781672 116 else if(c == 'x')
sype 37:da3a2c781672 117 {
IceTeam 39:309f38d1e49c 118 if (state != 5) {
IceTeam 39:309f38d1e49c 119 state = 5;
IceTeam 39:309f38d1e49c 120 roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
IceTeam 39:309f38d1e49c 121 }
sype 37:da3a2c781672 122 }
IceTeam 39:309f38d1e49c 123 else if (c == '\0') { ; }
IceTeam 39:309f38d1e49c 124 else {
IceTeam 39:309f38d1e49c 125 if (state != 0) {
IceTeam 39:309f38d1e49c 126 roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
IceTeam 39:309f38d1e49c 127 state = 0;
IceTeam 39:309f38d1e49c 128 }
IceTeam 39:309f38d1e49c 129 }
IceTeam 39:309f38d1e49c 130 // }
IceTeam 39:309f38d1e49c 131 // roboclaw.ForwardM1(ADR, 0);
IceTeam 39:309f38d1e49c 132 // roboclaw.ForwardM2(ADR, 0);
IceTeam 39:309f38d1e49c 133
IceTeam 39:309f38d1e49c 134 }*/
sype 2:abdf8c6823a1 135 }
sype 2:abdf8c6823a1 136
sype 2:abdf8c6823a1 137 void init(void)
sype 0:ad9600df4a70 138 {
sype 37:da3a2c781672 139 roboclaw.ForwardM1(ADR, 0);
sype 37:da3a2c781672 140 roboclaw.ForwardM2(ADR, 0);
sype 37:da3a2c781672 141 logger.baud(9600);
sype 37:da3a2c781672 142 logger.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 143 wait_ms(500);
sype 10:ae3178aa94e9 144
IceTeam 27:ae68960316f1 145 odo.setPos(0, 1000, 0);
sype 37:da3a2c781672 146
sype 10:ae3178aa94e9 147 while(button);
sype 10:ae3178aa94e9 148 wait(1);
sype 10:ae3178aa94e9 149 mybutton.fall(&pressed);
IceTeam 39:309f38d1e49c 150 mybutton.rise(&unpressed);
sype 0:ad9600df4a70 151 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 39:309f38d1e49c 152
IceTeam 39:309f38d1e49c 153 logger.printf("End init\n\r");
sype 0:ad9600df4a70 154 }
sype 0:ad9600df4a70 155
sype 0:ad9600df4a70 156 void update_main(void)
sype 0:ad9600df4a70 157 {
sype 0:ad9600df4a70 158 odo.update_odo();
sype 10:ae3178aa94e9 159 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 2:abdf8c6823a1 160 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 161 }
sype 10:ae3178aa94e9 162
sype 10:ae3178aa94e9 163 void pressed(void)
sype 10:ae3178aa94e9 164 {
sype 10:ae3178aa94e9 165 if(i==0) {
sype 10:ae3178aa94e9 166 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 167 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 168 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 169 i++;
sype 10:ae3178aa94e9 170 }
sype 10:ae3178aa94e9 171 }
sype 10:ae3178aa94e9 172
sype 10:ae3178aa94e9 173 void unpressed(void)
sype 10:ae3178aa94e9 174 {
sype 10:ae3178aa94e9 175 if(i==1) {
sype 10:ae3178aa94e9 176 i--;
sype 10:ae3178aa94e9 177 }
sype 10:ae3178aa94e9 178 }