ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

main.cpp@41:b5a2fbc20beb, 2016-04-13 (annotated)

Committer:
sype
Date:
Wed Apr 13 11:27:34 2016 +0000
Revision:
41:b5a2fbc20beb
Parent:
39:309f38d1e49c
Child:
43:d5aaff7d2bec
Impl?mentation des dispositifs du robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 41:b5a2fbc20beb 1 #include "func.h"
sype 0:ad9600df4a70 2
sype 0:ad9600df4a70 3 #define dt 10000
sype 10:ae3178aa94e9 4 #define ATTENTE 0
sype 10:ae3178aa94e9 5 #define GO 1
sype 10:ae3178aa94e9 6 #define STOP 2
sype 0:ad9600df4a70 7
sype 41:b5a2fbc20beb 8 /* Déclaration des différents éléments de l'IHM */
sype 10:ae3178aa94e9 9 InterruptIn mybutton(USER_BUTTON);
sype 10:ae3178aa94e9 10 DigitalIn button(USER_BUTTON);
sype 2:abdf8c6823a1 11 DigitalOut led(LED1);
sype 41:b5a2fbc20beb 12 DigitalOut bleu(PC_5);
sype 41:b5a2fbc20beb 13 DigitalOut blanc(PC_6);
sype 41:b5a2fbc20beb 14 DigitalOut rouge(PC_8);
sype 41:b5a2fbc20beb 15
sype 41:b5a2fbc20beb 16 /* AX12 */
sype 41:b5a2fbc20beb 17 AX12 left_hand(PA_15, PB_7, 4, 250000);
sype 41:b5a2fbc20beb 18 AX12 right_hand(PA_15, PB_7, 1, 250000);
sype 41:b5a2fbc20beb 19
sype 41:b5a2fbc20beb 20 /* Sharp */
sype 41:b5a2fbc20beb 21 AnalogIn capt1(PA_4);
sype 41:b5a2fbc20beb 22 AnalogIn capt2(PB_0);
sype 41:b5a2fbc20beb 23 AnalogIn capt3(PC_1);
sype 41:b5a2fbc20beb 24 AnalogIn capt4(PC_0);
sype 41:b5a2fbc20beb 25
sype 41:b5a2fbc20beb 26 /* Moteurs pas à pas */
sype 41:b5a2fbc20beb 27 Stepper RMot(NC, PB_10, PA_8);
sype 41:b5a2fbc20beb 28 Stepper ZMot(NC, PB_5, PB_4);
sype 41:b5a2fbc20beb 29 Stepper LMot(NC, PB_3, PA_10);
sype 41:b5a2fbc20beb 30 /* Fins de course */
sype 41:b5a2fbc20beb 31 InterruptIn EndR(PB_15);
sype 41:b5a2fbc20beb 32 InterruptIn EndZ(PB_14);
sype 41:b5a2fbc20beb 33 InterruptIn EndL(PB_13);
sype 41:b5a2fbc20beb 34
sype 0:ad9600df4a70 35 Ticker ticker;
sype 41:b5a2fbc20beb 36
sype 41:b5a2fbc20beb 37 Serial logger(USBTX, USBRX);
sype 41:b5a2fbc20beb 38 //Serial logger(PA_9, PA_10);
sype 10:ae3178aa94e9 39 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 39:309f38d1e49c 40 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
sype 10:ae3178aa94e9 41
sype 41:b5a2fbc20beb 42 int i = 0, state = 0;
sype 41:b5a2fbc20beb 43 bool EL = false, EZ = false, ER = false;
sype 0:ad9600df4a70 44
sype 0:ad9600df4a70 45 void update_main(void);
sype 2:abdf8c6823a1 46 void init(void);
sype 0:ad9600df4a70 47
IceTeam 9:e39b218ab20d 48 /** Debut du programme */
sype 0:ad9600df4a70 49 int main(void)
sype 10:ae3178aa94e9 50 {
sype 37:da3a2c781672 51 init();
IceTeam 39:309f38d1e49c 52 /* Code AStar */
sype 41:b5a2fbc20beb 53
sype 41:b5a2fbc20beb 54 /*double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 55 double distance_v = 200.0;
IceTeam 13:5355aed288b0 56 std::vector<SimplePoint> path;
IceTeam 13:5355aed288b0 57 Map map;
sype 41:b5a2fbc20beb 58
sype 41:b5a2fbc20beb 59
IceTeam 18:0f1fefe78266 60 //Construction des obstacles
IceTeam 36:2d7357a385bc 61 map.build();
sype 41:b5a2fbc20beb 62
IceTeam 36:2d7357a385bc 63 float x=odo.getX();
IceTeam 30:58bfac39e701 64 float y=odo.getY();
IceTeam 30:58bfac39e701 65 float the = 0;
sype 41:b5a2fbc20beb 66
IceTeam 39:309f38d1e49c 67 map.AStar(x, y, 1600, 1000, 75);
IceTeam 14:a394e27b8cb2 68 path = map.path;
sype 41:b5a2fbc20beb 69
IceTeam 18:0f1fefe78266 70 for(int i=0; i<path.size();i++) {
sype 41:b5a2fbc20beb 71 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
IceTeam 30:58bfac39e701 72 odo.GotoXYT(path[i].x, path[i].y, the);
IceTeam 27:ae68960316f1 73 }
sype 41:b5a2fbc20beb 74
sype 41:b5a2fbc20beb 75 map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
sype 41:b5a2fbc20beb 76 path = map.path;
sype 41:b5a2fbc20beb 77
sype 41:b5a2fbc20beb 78 for(int i=0; i<path.size();i++) {
sype 41:b5a2fbc20beb 79 the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
sype 41:b5a2fbc20beb 80 odo.GotoXYT(path[i].x, path[i].y, the);
sype 41:b5a2fbc20beb 81 }*/
sype 41:b5a2fbc20beb 82
IceTeam 36:2d7357a385bc 83 //odo.GotoThet(PI);
sype 41:b5a2fbc20beb 84 //odo.GotoThet(0);
IceTeam 36:2d7357a385bc 85 //odo.TestEntraxe(3);
sype 41:b5a2fbc20beb 86
sype 10:ae3178aa94e9 87 //odo.GotoThet(-PI/2);
sype 41:b5a2fbc20beb 88 //wait(2000);
IceTeam 39:309f38d1e49c 89 //odo.GotoXYT(2250,500,0);
sype 41:b5a2fbc20beb 90
IceTeam 39:309f38d1e49c 91 //odo.TestEntraxe(5);
IceTeam 39:309f38d1e49c 92 //odo.Forward(1000);
sype 37:da3a2c781672 93 while(1)
sype 37:da3a2c781672 94 {
sype 41:b5a2fbc20beb 95 JPO();
sype 41:b5a2fbc20beb 96 }
sype 2:abdf8c6823a1 97 }
sype 2:abdf8c6823a1 98
sype 2:abdf8c6823a1 99 void init(void)
sype 0:ad9600df4a70 100 {
sype 37:da3a2c781672 101 roboclaw.ForwardM1(ADR, 0);
sype 37:da3a2c781672 102 roboclaw.ForwardM2(ADR, 0);
sype 37:da3a2c781672 103 logger.baud(9600);
sype 37:da3a2c781672 104 logger.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 105 wait_ms(500);
sype 41:b5a2fbc20beb 106 bleu = 1;
sype 41:b5a2fbc20beb 107 blanc = 1;
sype 41:b5a2fbc20beb 108 rouge = 1;
sype 41:b5a2fbc20beb 109 wait_ms(1000);
sype 41:b5a2fbc20beb 110 bleu = 0;
sype 41:b5a2fbc20beb 111 blanc = 0;
sype 41:b5a2fbc20beb 112 rouge = 0;
sype 10:ae3178aa94e9 113
sype 41:b5a2fbc20beb 114 //odo.setPos(0, 1000, 0);
sype 10:ae3178aa94e9 115 while(button);
sype 10:ae3178aa94e9 116 wait(1);
sype 41:b5a2fbc20beb 117
sype 10:ae3178aa94e9 118 mybutton.fall(&pressed);
IceTeam 39:309f38d1e49c 119 mybutton.rise(&unpressed);
sype 41:b5a2fbc20beb 120
sype 41:b5a2fbc20beb 121 EndL.fall(&ELpressed);
sype 41:b5a2fbc20beb 122 EndL.rise(&ELunpressed);
sype 41:b5a2fbc20beb 123 EndZ.fall(&EZpressed);
sype 41:b5a2fbc20beb 124 EndZ.rise(&EZunpressed);
sype 41:b5a2fbc20beb 125 EndR.fall(&ERpressed);
sype 41:b5a2fbc20beb 126 EndR.rise(&ERunpressed);
sype 41:b5a2fbc20beb 127
sype 41:b5a2fbc20beb 128 wait_ms(100);
sype 0:ad9600df4a70 129 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 39:309f38d1e49c 130 logger.printf("End init\n\r");
sype 0:ad9600df4a70 131 }
sype 0:ad9600df4a70 132
sype 0:ad9600df4a70 133 void update_main(void)
sype 0:ad9600df4a70 134 {
sype 0:ad9600df4a70 135 odo.update_odo();
sype 41:b5a2fbc20beb 136 checkAround();
sype 10:ae3178aa94e9 137 }