Robot's source code
Dependencies: mbed
main.cpp@94:5c37bcf73d14, 2015-04-30 (annotated)
- Committer:
- Near32
- Date:
- Thu Apr 30 16:03:55 2015 +0000
- Revision:
- 94:5c37bcf73d14
- Parent:
- 85:8e95432d99d3
- Child:
- 99:9f4f3e46c2f0
maj asservA via local
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jagang | 0:41149573d577 | 1 | #include "mbed.h" |
Jagang | 39:09c04fd42c94 | 2 | |
Near32 | 94:5c37bcf73d14 | 3 | #define PLAN_A |
Jagang | 74:88be86f83d17 | 4 | |
Jagang | 39:09c04fd42c94 | 5 | #include "defines.h" |
Jagang | 39:09c04fd42c94 | 6 | |
Jagang | 0:41149573d577 | 7 | #include "QEI.h" |
Jagang | 23:228317fe0457 | 8 | #include "Map.h" |
Jagang | 39:09c04fd42c94 | 9 | #include "AX12.h" |
Jagang | 0:41149573d577 | 10 | |
Jagang | 71:95d76c181b22 | 11 | #ifdef PLAN_A |
Jagang | 71:95d76c181b22 | 12 | #include "Odometry.h" |
Jagang | 71:95d76c181b22 | 13 | #include "Asserv.h" |
Jagang | 71:95d76c181b22 | 14 | #else |
Jagang | 71:95d76c181b22 | 15 | #include "Odometry2.h" |
Jagang | 71:95d76c181b22 | 16 | #include "planB.h" |
Jagang | 71:95d76c181b22 | 17 | #endif |
Jagang | 71:95d76c181b22 | 18 | |
Near32 | 36:54f86bc6fd80 | 19 | #include "Motor.h" |
Near32 | 17:f360e21d3307 | 20 | /*----------------------------------------------------------------------------------------------*/ |
Jagang | 39:09c04fd42c94 | 21 | /*Serial*/ |
sype | 80:5399183aa39b | 22 | |
sype | 84:24d727006218 | 23 | //Serial logger(OUT_TX, OUT_RX); // tx, rx |
sype | 84:24d727006218 | 24 | Serial logger(USBTX,USBRX); |
Near32 | 48:cb3ebbc27db3 | 25 | //logger.baud((int)115200); |
Near32 | 17:f360e21d3307 | 26 | /*----------------------------------------------------------------------------------------------*/ |
Jagang | 21:5443f93819db | 27 | |
Jagang | 23:228317fe0457 | 28 | /* --- Initialisation de la liste des obstable --- */ |
Jagang | 23:228317fe0457 | 29 | int Obstacle::lastId = 0; |
Jagang | 23:228317fe0457 | 30 | |
Jagang | 0:41149573d577 | 31 | int main() |
Jagang | 0:41149573d577 | 32 | { |
Jagang | 39:09c04fd42c94 | 33 | logger.printf("Initialisation...\r\n"); |
Near32 | 36:54f86bc6fd80 | 34 | |
Jagang | 78:7c7cefbe1772 | 35 | AX12 test(AX12_TX,AX12_RX,5,250000); |
Jagang | 74:88be86f83d17 | 36 | test.setMode(0); |
Jagang | 74:88be86f83d17 | 37 | |
Near32 | 94:5c37bcf73d14 | 38 | #ifdef PLAN_B |
Near32 | 94:5c37bcf73d14 | 39 | while(1) |
Jagang | 74:88be86f83d17 | 40 | { |
Jagang | 74:88be86f83d17 | 41 | test.setMaxTorque(0x1FF); |
Jagang | 74:88be86f83d17 | 42 | test.setGoal(0); |
Jagang | 74:88be86f83d17 | 43 | logger.printf("0\r\n"); |
Jagang | 74:88be86f83d17 | 44 | wait(2); |
Jagang | 74:88be86f83d17 | 45 | test.setMaxTorque(0x1FF); |
Jagang | 74:88be86f83d17 | 46 | test.setGoal(90); |
Jagang | 74:88be86f83d17 | 47 | logger.printf("180\r\n"); |
Jagang | 74:88be86f83d17 | 48 | wait(4); |
Near32 | 94:5c37bcf73d14 | 49 | } |
Near32 | 94:5c37bcf73d14 | 50 | #endif |
Jagang | 74:88be86f83d17 | 51 | |
Jagang | 40:83ce8d1072ef | 52 | PwmOut pw1(PWM_MOT1); |
Jagang | 40:83ce8d1072ef | 53 | DigitalOut dir1(DIR_MOT1); |
Jagang | 40:83ce8d1072ef | 54 | PwmOut pw2(PWM_MOT2); |
Jagang | 40:83ce8d1072ef | 55 | DigitalOut dir2(DIR_MOT2); |
Near32 | 17:f360e21d3307 | 56 | |
Near32 | 36:54f86bc6fd80 | 57 | |
Near32 | 48:cb3ebbc27db3 | 58 | Motor motorL(PWM_MOT1,DIR_MOT1); |
Near32 | 48:cb3ebbc27db3 | 59 | Motor motorR(PWM_MOT2,DIR_MOT2); |
Jagang | 57:ab13f4e7a2b2 | 60 | |
Jagang | 39:09c04fd42c94 | 61 | Timer t; |
Jagang | 39:09c04fd42c94 | 62 | |
Jagang | 63:fd9af0693e50 | 63 | //AX12 test(PA_9,NC,0x03,250000); |
Jagang | 54:e0e58c36658a | 64 | |
Jagang | 39:09c04fd42c94 | 65 | AnalogIn ain0(PA_0); |
Jagang | 39:09c04fd42c94 | 66 | AnalogIn ain1(PA_1); |
Jagang | 39:09c04fd42c94 | 67 | AnalogIn ain2(PA_4); |
Jagang | 39:09c04fd42c94 | 68 | AnalogIn ain3(PB_0); |
Jagang | 39:09c04fd42c94 | 69 | AnalogIn ain4(PC_1); |
Near32 | 24:3c0422e1ebd6 | 70 | |
Near32 | 3:573a0dc8383f | 71 | /*----------------------------------------------------------------------------------------------*/ |
Near32 | 3:573a0dc8383f | 72 | /*Odometry*/ |
Jagang | 71:95d76c181b22 | 73 | #ifdef PLAN_A |
Jagang | 71:95d76c181b22 | 74 | QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING); |
Jagang | 71:95d76c181b22 | 75 | QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING); |
Jagang | 71:95d76c181b22 | 76 | |
sype | 79:d97090bb6470 | 77 | Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255); |
Jagang | 78:7c7cefbe1772 | 78 | odometry.setTheta(0); |
Jagang | 78:7c7cefbe1772 | 79 | odometry.setX(0); |
Jagang | 78:7c7cefbe1772 | 80 | odometry.setY(0); |
Jagang | 71:95d76c181b22 | 81 | #else |
Jagang | 71:95d76c181b22 | 82 | QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING); |
Jagang | 71:95d76c181b22 | 83 | QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING); |
Jagang | 71:95d76c181b22 | 84 | |
sype | 79:d97090bb6470 | 85 | Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255); |
Jagang | 71:95d76c181b22 | 86 | |
Jagang | 71:95d76c181b22 | 87 | odometry.setTheta(PI/2); |
Jagang | 71:95d76c181b22 | 88 | odometry.setX(0); |
Jagang | 71:95d76c181b22 | 89 | odometry.setY(0); |
Jagang | 71:95d76c181b22 | 90 | #endif |
Jagang | 60:546d7b43333b | 91 | |
Jagang | 63:fd9af0693e50 | 92 | DigitalOut led1(LED1); |
Jagang | 63:fd9af0693e50 | 93 | DigitalOut led2(LED2); |
Jagang | 63:fd9af0693e50 | 94 | DigitalOut led3(LED3); |
Jagang | 39:09c04fd42c94 | 95 | |
Jagang | 71:95d76c181b22 | 96 | bool testOdo = false; |
Jagang | 71:95d76c181b22 | 97 | |
Jagang | 71:95d76c181b22 | 98 | #ifdef PLAN_A |
Jagang | 71:95d76c181b22 | 99 | Asserv<float> instanceAsserv(&odometry); |
Near32 | 94:5c37bcf73d14 | 100 | //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0); |
Near32 | 94:5c37bcf73d14 | 101 | instanceAsserv.setGoal(300.0f,200.0f,0.0f); |
Near32 | 94:5c37bcf73d14 | 102 | logger.printf("GOAL SET... RUNNING!\r\n"); |
Jagang | 71:95d76c181b22 | 103 | |
Jagang | 74:88be86f83d17 | 104 | char state = 0; |
Jagang | 74:88be86f83d17 | 105 | |
Jagang | 71:95d76c181b22 | 106 | t.start(); |
Jagang | 63:fd9af0693e50 | 107 | t.reset(); |
Jagang | 63:fd9af0693e50 | 108 | |
Jagang | 71:95d76c181b22 | 109 | while(!testOdo) |
Jagang | 71:95d76c181b22 | 110 | { |
Jagang | 71:95d76c181b22 | 111 | float dt = t.read(); |
Jagang | 71:95d76c181b22 | 112 | t.reset(); |
Jagang | 74:88be86f83d17 | 113 | |
Jagang | 71:95d76c181b22 | 114 | odometry.update(dt); |
Jagang | 74:88be86f83d17 | 115 | instanceAsserv.update(dt); |
Near32 | 73:d8e1b543fbe3 | 116 | float phi_r = (float)instanceAsserv.getPhiR(); |
Near32 | 73:d8e1b543fbe3 | 117 | float phi_l = (float)instanceAsserv.getPhiL(); |
Jagang | 74:88be86f83d17 | 118 | float phi_max = (float)instanceAsserv.getPhiMax(); |
Near32 | 94:5c37bcf73d14 | 119 | |
Jagang | 71:95d76c181b22 | 120 | |
Near32 | 94:5c37bcf73d14 | 121 | motorR.setSpeed(0.08f+(phi_r/phi_max)); |
Near32 | 94:5c37bcf73d14 | 122 | motorL.setSpeed(0.08f+(phi_l/phi_max)); |
Near32 | 94:5c37bcf73d14 | 123 | //#define test |
Near32 | 81:08eacb6179d8 | 124 | #ifdef test |
Jagang | 76:a0c09fd62be1 | 125 | if(state == 0 && instanceAsserv.isArrivedRho()) |
Jagang | 76:a0c09fd62be1 | 126 | { |
Jagang | 76:a0c09fd62be1 | 127 | state = 1; |
Near32 | 94:5c37bcf73d14 | 128 | logger.printf("Arrive en 0,0 !!!!!\r\n"); |
Jagang | 76:a0c09fd62be1 | 129 | wait(5); |
Near32 | 94:5c37bcf73d14 | 130 | instanceAsserv.setGoal(300.0f,200.0f,0.0f); |
Jagang | 76:a0c09fd62be1 | 131 | } |
Jagang | 76:a0c09fd62be1 | 132 | else if(state == 1 && instanceAsserv.isArrivedRho()) |
Jagang | 76:a0c09fd62be1 | 133 | { |
Jagang | 76:a0c09fd62be1 | 134 | state = 2; |
Near32 | 94:5c37bcf73d14 | 135 | logger.printf("Arrive en 200,200 !!!!!\r\n"); |
Jagang | 76:a0c09fd62be1 | 136 | wait(5); |
Near32 | 94:5c37bcf73d14 | 137 | instanceAsserv.setGoal(0.0f,300.0f,0.0f); |
Jagang | 76:a0c09fd62be1 | 138 | } |
Jagang | 76:a0c09fd62be1 | 139 | else if(state == 2 && instanceAsserv.isArrivedRho()) |
Jagang | 76:a0c09fd62be1 | 140 | { |
Jagang | 76:a0c09fd62be1 | 141 | state = 0; |
Near32 | 94:5c37bcf73d14 | 142 | logger.printf("Arrive en 0,200 !!!!!\r\n"); |
Jagang | 76:a0c09fd62be1 | 143 | wait(5); |
Near32 | 94:5c37bcf73d14 | 144 | instanceAsserv.setGoal(0.0f,0.0f,0.0f); |
Jagang | 76:a0c09fd62be1 | 145 | } |
Near32 | 81:08eacb6179d8 | 146 | #endif |
Near32 | 94:5c37bcf73d14 | 147 | |
Jagang | 71:95d76c181b22 | 148 | } |
Jagang | 71:95d76c181b22 | 149 | #else |
Jagang | 71:95d76c181b22 | 150 | aserv_planB asserv(odometry,motorL,motorR); |
Near32 | 94:5c37bcf73d14 | 151 | asserv.setGoal(1,-5000,0); |
Jagang | 71:95d76c181b22 | 152 | |
Jagang | 71:95d76c181b22 | 153 | t.start(); |
Jagang | 63:fd9af0693e50 | 154 | t.reset(); |
Jagang | 71:95d76c181b22 | 155 | |
Jagang | 71:95d76c181b22 | 156 | while(!testOdo) |
Jagang | 71:95d76c181b22 | 157 | { |
Jagang | 71:95d76c181b22 | 158 | //Parametrage des coef par bluetooth |
Near32 | 94:5c37bcf73d14 | 159 | while(logger.readable()) { |
Jagang | 71:95d76c181b22 | 160 | char c = logger.getc(); |
sype | 79:d97090bb6470 | 161 | if(c=='a') asserv.Kd += 0.001; |
sype | 79:d97090bb6470 | 162 | else if(c=='z') asserv.Kd -= 0.001; |
sype | 79:d97090bb6470 | 163 | logger.printf("%f\n\r",asserv.Kd); |
Near32 | 94:5c37bcf73d14 | 164 | } |
Jagang | 71:95d76c181b22 | 165 | |
Jagang | 71:95d76c181b22 | 166 | //Asservissement : |
Jagang | 71:95d76c181b22 | 167 | float dt = t.read(); |
Jagang | 71:95d76c181b22 | 168 | t.reset(); |
Jagang | 71:95d76c181b22 | 169 | odometry.update(dt); |
Jagang | 71:95d76c181b22 | 170 | asserv.update(dt); |
Near32 | 94:5c37bcf73d14 | 171 | |
Near32 | 94:5c37bcf73d14 | 172 | wait_ms(100); |
Jagang | 71:95d76c181b22 | 173 | } |
Jagang | 71:95d76c181b22 | 174 | #endif |
Near32 | 43:87bdce65341f | 175 | } |