Yannick OMNES / Mbed 2 deprecated 2-FisherMan

azert

Dependencies:   AX12 Servo VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed

Fork of 2-FisherMan by Julien Tiron

main.cpp@3:387812a9386a, 2016-05-06 (annotated)

Committer:
Yannick292
Date:
Fri May 06 07:42:51 2016 +0000
Revision:
3:387812a9386a
Parent:
2:bbd254222dd7
Child:
4:0e9f65e3e2a0
update mooves;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julientiron 0:88fd29503679 1 /**
julientiron 0:88fd29503679 2 ******************************************************************************
julientiron 0:88fd29503679 3 * @file main.cpp
julientiron 0:88fd29503679 4 * @author Julien Tiron, FIP Télécom Bretagne
julientiron 0:88fd29503679 5 * @version V1.0.0
julientiron 0:88fd29503679 6 * @date March 23th, 2016
julientiron 0:88fd29503679 7 * @brief DoorCloser robot main code
julientiron 0:88fd29503679 8 ******************************************************************************
julientiron 0:88fd29503679 9 **/
julientiron 0:88fd29503679 10
julientiron 0:88fd29503679 11 /* Includes ------------------------------------------------------------------*/
julientiron 0:88fd29503679 12
julientiron 0:88fd29503679 13 #include "mbed.h"
julientiron 0:88fd29503679 14 #include "DevSPI.h"
julientiron 0:88fd29503679 15 #include "l6474_class.h"
julientiron 0:88fd29503679 16 #include "DevI2C.h"
julientiron 0:88fd29503679 17 #include "VL6180x.h"
julientiron 0:88fd29503679 18 #include "AX12.h"
julientiron 0:88fd29503679 19 #include "Servo.h"
julientiron 0:88fd29503679 20 /* Definitions ---------------------------------------------------------------*/
julientiron 0:88fd29503679 21
julientiron 0:88fd29503679 22 #define VL6180X_ADDRESS 0x29
julientiron 0:88fd29503679 23
julientiron 0:88fd29503679 24 /* Variables -----------------------------------------------------------------*/
julientiron 0:88fd29503679 25
julientiron 0:88fd29503679 26 /* Start and Stop Component */
julientiron 0:88fd29503679 27 InterruptIn startup(PC_1);
julientiron 0:88fd29503679 28 Ticker pwm_ticker;
julientiron 0:88fd29503679 29 Timeout game_length;
julientiron 0:88fd29503679 30 volatile bool start = 1;
julientiron 0:88fd29503679 31 volatile bool end = 1;
julientiron 0:88fd29503679 32 char data = 0x08 | (char)64;
Yannick292 1:5c28b22f3ca0 33 int step=0;
julientiron 0:88fd29503679 34 bool tag = true;
julientiron 0:88fd29503679 35 int i2cAddres=0x70; // Address of DS1307 is 0x68 (7 bit address)
julientiron 0:88fd29503679 36 int i2c8BitAddres= i2cAddres <<1; // Convert to 8bit addressing used by mbed
julientiron 0:88fd29503679 37 Servo myservo(D5);
julientiron 0:88fd29503679 38 PwmOut parasol(A3);
julientiron 0:88fd29503679 39 /* Motor Control Component */
julientiron 0:88fd29503679 40 L6474 *motor1;
julientiron 0:88fd29503679 41 L6474 *motor2;
julientiron 0:88fd29503679 42 DigitalInOut sdaDummy(D14);
julientiron 0:88fd29503679 43 DigitalInOut sclDummy(D15);
julientiron 0:88fd29503679 44 /* Distance Sensors Component */
julientiron 0:88fd29503679 45 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
julientiron 0:88fd29503679 46
julientiron 0:88fd29503679 47 /* Functions -----------------------------------------------------------------*/
julientiron 0:88fd29503679 48
julientiron 0:88fd29503679 49 void go()
julientiron 0:88fd29503679 50 {
julientiron 0:88fd29503679 51 start = 0;
julientiron 0:88fd29503679 52 }
julientiron 0:88fd29503679 53
julientiron 0:88fd29503679 54 void stop()
julientiron 0:88fd29503679 55 {
julientiron 0:88fd29503679 56 end = 0;
julientiron 0:88fd29503679 57 }
julientiron 0:88fd29503679 58
julientiron 0:88fd29503679 59 void init_sensor()
julientiron 0:88fd29503679 60 {
julientiron 0:88fd29503679 61
julientiron 0:88fd29503679 62 }
julientiron 0:88fd29503679 63
julientiron 0:88fd29503679 64 void switch_sensor(int number)
julientiron 0:88fd29503679 65 {
julientiron 0:88fd29503679 66
julientiron 0:88fd29503679 67 }
julientiron 0:88fd29503679 68
julientiron 0:88fd29503679 69 void pwm();
julientiron 0:88fd29503679 70
julientiron 0:88fd29503679 71 void deplacement(int distanceEnCm, bool sens)
julientiron 0:88fd29503679 72 {
julientiron 0:88fd29503679 73 int nbPas = distanceEnCm*166.7;
julientiron 0:88fd29503679 74 if(sens) {
julientiron 0:88fd29503679 75 motor1->Move(StepperMotor::BWD,nbPas);
julientiron 0:88fd29503679 76 motor2->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 77 } else {
julientiron 0:88fd29503679 78 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 79 motor2->Move(StepperMotor::BWD,nbPas);
julientiron 0:88fd29503679 80 }
julientiron 0:88fd29503679 81 motor1->WaitWhileActive();
julientiron 0:88fd29503679 82 motor2->WaitWhileActive();
julientiron 0:88fd29503679 83 motor1->HardStop();
julientiron 0:88fd29503679 84 motor2->HardStop();
julientiron 0:88fd29503679 85 }
julientiron 0:88fd29503679 86
julientiron 0:88fd29503679 87 void rotation(int angleEnDegre, int diametre)
julientiron 0:88fd29503679 88 {
julientiron 0:88fd29503679 89 int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
julientiron 0:88fd29503679 90 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 91 nbPas /= 990;
julientiron 0:88fd29503679 92
julientiron 0:88fd29503679 93 /*
julientiron 0:88fd29503679 94 int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
julientiron 0:88fd29503679 95 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 96 nbPas *= 2;*/
julientiron 0:88fd29503679 97
julientiron 0:88fd29503679 98 printf("%d \n\r", nbPas);
julientiron 0:88fd29503679 99 motor2->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 100 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 101 motor1->WaitWhileActive();
julientiron 0:88fd29503679 102 motor2->WaitWhileActive();
julientiron 0:88fd29503679 103 motor1->HardStop();
julientiron 0:88fd29503679 104 motor2->HardStop();
julientiron 0:88fd29503679 105 }
julientiron 0:88fd29503679 106
julientiron 0:88fd29503679 107
julientiron 0:88fd29503679 108 /* Main ----------------------------------------------------------------------*/
julientiron 0:88fd29503679 109
julientiron 0:88fd29503679 110 int main()
julientiron 0:88fd29503679 111 {
julientiron 0:88fd29503679 112 /*----- Initialization. -----*/
julientiron 0:88fd29503679 113
julientiron 0:88fd29503679 114 /* Initializing SPI bus. */
julientiron 0:88fd29503679 115 DevSPI dev_spi(D11, D12, D13);
julientiron 0:88fd29503679 116 myservo.calibrate(0.00095, 90.0); // Calibrate the servo
julientiron 0:88fd29503679 117 parasol.period_ms(10);
julientiron 0:88fd29503679 118 parasol.pulsewidth_ms(1);
julientiron 0:88fd29503679 119 /* Initializing Motor Control Components. */
julientiron 0:88fd29503679 120 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
julientiron 0:88fd29503679 121 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
julientiron 0:88fd29503679 122 if (motor1->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 123 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 124 if (motor2->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 125 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 126 sdaDummy.mode(PullUp);
julientiron 0:88fd29503679 127 sclDummy.mode(PullUp);
julientiron 0:88fd29503679 128 //pwm_ticker.attach(&pwm, 0.1);
julientiron 0:88fd29503679 129 /* Initializing AX-12 motors */
julientiron 0:88fd29503679 130 /* AX12 ax12_1 (D1, D0, 1, 1000000);
julientiron 0:88fd29503679 131 AX12 ax12_2 (D1, D0, 2, 1000000);
julientiron 0:88fd29503679 132 AX12 ax12_3 (D1, D0, 3, 1000000);
julientiron 0:88fd29503679 133 AX12 ax12_4 (D1, D0, 4, 1000000);
julientiron 0:88fd29503679 134 AX12 ax12_5 (D1, D0, 5, 1000000);*/
julientiron 0:88fd29503679 135
julientiron 0:88fd29503679 136 /* Interrupt to start the robot */
julientiron 0:88fd29503679 137 startup.fall(&go);
julientiron 0:88fd29503679 138 /*motor1->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 139 motor2->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 140 motor1->WaitWhileActive();
julientiron 0:88fd29503679 141 motor2->WaitWhileActive();
julientiron 0:88fd29503679 142 motor1->HardStop();
julientiron 0:88fd29503679 143 motor2->HardStop();*/
julientiron 0:88fd29503679 144 //deplacement(10, 0);
julientiron 0:88fd29503679 145 /* rotation(90, 30);
julientiron 0:88fd29503679 146 wait(2);
julientiron 0:88fd29503679 147 rotation(45, 30);
julientiron 0:88fd29503679 148 wait(2);
julientiron 0:88fd29503679 149 rotation(180, 30);
julientiron 0:88fd29503679 150 wait(2);
julientiron 0:88fd29503679 151 rotation(45, 30);*/
julientiron 0:88fd29503679 152 /*motor1->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 153 motor2->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 154 motor1->WaitWhileActive();
julientiron 0:88fd29503679 155 motor2->WaitWhileActive();
julientiron 0:88fd29503679 156 motor1->HardStop();
julientiron 0:88fd29503679 157 motor2->HardStop();
julientiron 0:88fd29503679 158 */
julientiron 0:88fd29503679 159
julientiron 0:88fd29503679 160 while(start) {
julientiron 0:88fd29503679 161 /* Waiting code */
julientiron 0:88fd29503679 162 /* ax12_1.SetSpeed(70);
julientiron 0:88fd29503679 163 ax12_2.SetSpeed(70);
julientiron 0:88fd29503679 164 ax12_3.SetSpeed(70);
julientiron 0:88fd29503679 165 ax12_4.SetSpeed(70);
julientiron 0:88fd29503679 166 ax12_5.SetSpeed(70);*/
julientiron 0:88fd29503679 167 //wait_ms(100);
julientiron 0:88fd29503679 168 }
julientiron 0:88fd29503679 169
julientiron 0:88fd29503679 170 /* Interrupt to stop the robot */
julientiron 0:88fd29503679 171 game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
julientiron 0:88fd29503679 172
julientiron 0:88fd29503679 173 while(end) {
Yannick292 1:5c28b22f3ca0 174
Yannick292 1:5c28b22f3ca0 175 switch(step) {
Yannick292 1:5c28b22f3ca0 176 case 0:
Yannick292 1:5c28b22f3ca0 177 step = 1;
Yannick292 1:5c28b22f3ca0 178 break;
Yannick292 1:5c28b22f3ca0 179
Yannick292 1:5c28b22f3ca0 180 case 1:
Yannick292 1:5c28b22f3ca0 181 //Déplacement 1ers cubes au milieu
Yannick292 1:5c28b22f3ca0 182 wait_ms(100);
Yannick292 1:5c28b22f3ca0 183 motor1->Move(StepperMotor::BWD,12000);
Yannick292 1:5c28b22f3ca0 184 motor2->Move(StepperMotor::FWD,12000);
Yannick292 1:5c28b22f3ca0 185 while((sensor.getDistance()>50) && end) {
Yannick292 1:5c28b22f3ca0 186 //Tant que la distance est superieure on continue d'avancer
Yannick292 1:5c28b22f3ca0 187 wait_ms(20);
Yannick292 1:5c28b22f3ca0 188 printf("1");
Yannick292 1:5c28b22f3ca0 189 }
Yannick292 1:5c28b22f3ca0 190 step = 2;
Yannick292 1:5c28b22f3ca0 191 break;
Yannick292 1:5c28b22f3ca0 192
Yannick292 1:5c28b22f3ca0 193 case 2:
Yannick292 2:bbd254222dd7 194 motor2->Move(StepperMotor::FWD,2100); //première rotation 90°
Yannick292 2:bbd254222dd7 195 motor1->Move(StepperMotor::FWD,2100);
Yannick292 2:bbd254222dd7 196 motor1->WaitWhileActive();
Yannick292 2:bbd254222dd7 197 motor2->WaitWhileActive();
Yannick292 2:bbd254222dd7 198 motor1->HardStop();
Yannick292 2:bbd254222dd7 199 motor2->HardStop();
Yannick292 2:bbd254222dd7 200
Yannick292 2:bbd254222dd7 201
Yannick292 2:bbd254222dd7 202 motor2->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
Yannick292 2:bbd254222dd7 203 motor1->Move(StepperMotor::FWD,6000);
Yannick292 2:bbd254222dd7 204 motor1->WaitWhileActive();
Yannick292 2:bbd254222dd7 205 motor2->WaitWhileActive();
Yannick292 2:bbd254222dd7 206 motor1->HardStop();
Yannick292 2:bbd254222dd7 207 motor2->HardStop();
Yannick292 2:bbd254222dd7 208
Yannick292 3:387812a9386a 209 motor2->Move(StepperMotor::FWD,10); //recul pour rotation
Yannick292 3:387812a9386a 210 motor1->Move(StepperMotor::BWD,10);
Yannick292 3:387812a9386a 211 motor1->WaitWhileActive();
Yannick292 3:387812a9386a 212 motor2->WaitWhileActive();
Yannick292 3:387812a9386a 213 motor1->HardStop();
Yannick292 3:387812a9386a 214 motor2->HardStop();
Yannick292 3:387812a9386a 215
Yannick292 2:bbd254222dd7 216
Yannick292 2:bbd254222dd7 217 motor2->Move(StepperMotor::FWD,2100); //deuxième rotation 90° (même sens que première)
Yannick292 2:bbd254222dd7 218 motor1->Move(StepperMotor::FWD,2100);
Yannick292 2:bbd254222dd7 219 motor1->WaitWhileActive();
Yannick292 2:bbd254222dd7 220 motor2->WaitWhileActive();
Yannick292 2:bbd254222dd7 221 motor1->HardStop();
Yannick292 2:bbd254222dd7 222 motor2->HardStop();
Yannick292 2:bbd254222dd7 223
Yannick292 2:bbd254222dd7 224
Yannick292 2:bbd254222dd7 225 motor2->Move(StepperMotor::BWD,6000); //mise en position
Yannick292 2:bbd254222dd7 226 motor1->Move(StepperMotor::FWD,6000);
Yannick292 2:bbd254222dd7 227 motor1->WaitWhileActive();
Yannick292 2:bbd254222dd7 228 motor2->WaitWhileActive();
Yannick292 2:bbd254222dd7 229 motor1->HardStop();
Yannick292 2:bbd254222dd7 230 motor2->HardStop();
Yannick292 2:bbd254222dd7 231
Yannick292 3:387812a9386a 232 motor2->Move(StepperMotor::BWD,2100); //troisième rotation 90°
Yannick292 2:bbd254222dd7 233 motor1->Move(StepperMotor::BWD,2100);
Yannick292 2:bbd254222dd7 234 motor1->WaitWhileActive();
Yannick292 2:bbd254222dd7 235 motor2->WaitWhileActive();
Yannick292 2:bbd254222dd7 236 motor1->HardStop();
Yannick292 2:bbd254222dd7 237 motor2->HardStop();
Yannick292 2:bbd254222dd7 238
Yannick292 3:387812a9386a 239
Yannick292 3:387812a9386a 240 motor2->Move(StepperMotor::BWD,20); //Collage mur
Yannick292 3:387812a9386a 241 motor1->Move(StepperMotor::FWD,20);
Yannick292 3:387812a9386a 242 motor1->WaitWhileActive();
Yannick292 3:387812a9386a 243 motor2->WaitWhileActive();
Yannick292 3:387812a9386a 244 motor1->HardStop();
Yannick292 3:387812a9386a 245 motor2->HardStop();
Yannick292 3:387812a9386a 246
Yannick292 3:387812a9386a 247
Yannick292 3:387812a9386a 248 motor2->Move(StepperMotor::FWD,10); //recul pour rotation
Yannick292 3:387812a9386a 249 motor1->Move(StepperMotor::BWD,10);
Yannick292 3:387812a9386a 250 motor1->WaitWhileActive();
Yannick292 3:387812a9386a 251 motor2->WaitWhileActive();
Yannick292 3:387812a9386a 252 motor1->HardStop();
Yannick292 3:387812a9386a 253 motor2->HardStop();
Yannick292 3:387812a9386a 254
Yannick292 3:387812a9386a 255 motor2->Move(StepperMotor::BWD,2100); //quatrième rotation 90°
Yannick292 3:387812a9386a 256 motor1->Move(StepperMotor::BWD,2100);
Yannick292 3:387812a9386a 257 motor1->WaitWhileActive();
Yannick292 3:387812a9386a 258 motor2->WaitWhileActive();
Yannick292 3:387812a9386a 259 motor1->HardStop();
Yannick292 3:387812a9386a 260 motor2->HardStop();
Yannick292 3:387812a9386a 261
Yannick292 3:387812a9386a 262
Yannick292 3:387812a9386a 263
Yannick292 2:bbd254222dd7 264 //Mise en position du robot pour pêche
Yannick292 1:5c28b22f3ca0 265
Yannick292 2:bbd254222dd7 266 step = 3;
Yannick292 2:bbd254222dd7 267 break;
Yannick292 2:bbd254222dd7 268 case 3:
Yannick292 2:bbd254222dd7 269 //déploiement des bras, pêche
Yannick292 1:5c28b22f3ca0 270
Yannick292 2:bbd254222dd7 271 step = 4;
Yannick292 2:bbd254222dd7 272 break;
Yannick292 2:bbd254222dd7 273 case 4:
Yannick292 2:bbd254222dd7 274 //Dépose des poissons dans le filet
Yannick292 1:5c28b22f3ca0 275
Yannick292 2:bbd254222dd7 276 step = 5;
Yannick292 2:bbd254222dd7 277
Yannick292 2:bbd254222dd7 278 case 5:
Yannick292 2:bbd254222dd7 279 end = 0;
Yannick292 2:bbd254222dd7 280 break;
Yannick292 1:5c28b22f3ca0 281
Yannick292 1:5c28b22f3ca0 282
julientiron 0:88fd29503679 283 }
Yannick292 2:bbd254222dd7 284 }
julientiron 0:88fd29503679 285
Yannick292 2:bbd254222dd7 286 parasol.write(0.5);
Yannick292 2:bbd254222dd7 287 printf("2");
Yannick292 2:bbd254222dd7 288 motor1->HardStop();
Yannick292 2:bbd254222dd7 289 motor2->HardStop();
julientiron 0:88fd29503679 290 }
julientiron 0:88fd29503679 291
julientiron 0:88fd29503679 292 void pwm()
julientiron 0:88fd29503679 293 {
julientiron 0:88fd29503679 294 }