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@4:0e9f65e3e2a0, 2016-05-06 (annotated)

Committer:
Yannick292
Date:
Fri May 06 18:04:47 2016 +0000
Revision:
4:0e9f65e3e2a0
Parent:
3:387812a9386a 
ramassage de poiscaille;

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 }
Yannick292 4:0e9f65e3e2a0 81 /*while((sensor.getDistance()>50) && end) {
Yannick292 4:0e9f65e3e2a0 82 //Tant que la distance est superieure on continue d'avancer
Yannick292 4:0e9f65e3e2a0 83 wait_ms(20);
Yannick292 4:0e9f65e3e2a0 84 printf("1");
Yannick292 4:0e9f65e3e2a0 85 }*/
julientiron 0:88fd29503679 86 motor1->WaitWhileActive();
julientiron 0:88fd29503679 87 motor2->WaitWhileActive();
julientiron 0:88fd29503679 88 motor1->HardStop();
julientiron 0:88fd29503679 89 motor2->HardStop();
julientiron 0:88fd29503679 90 }
julientiron 0:88fd29503679 91
Yannick292 4:0e9f65e3e2a0 92
Yannick292 4:0e9f65e3e2a0 93 void rotation(int angleEnDegre, int diametre, int sens)
julientiron 0:88fd29503679 94 {
julientiron 0:88fd29503679 95 int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
julientiron 0:88fd29503679 96 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 97 nbPas /= 990;
julientiron 0:88fd29503679 98
julientiron 0:88fd29503679 99 /*
julientiron 0:88fd29503679 100 int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
julientiron 0:88fd29503679 101 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 102 nbPas *= 2;*/
julientiron 0:88fd29503679 103
julientiron 0:88fd29503679 104 printf("%d \n\r", nbPas);
Yannick292 4:0e9f65e3e2a0 105 if(sens) {
Yannick292 4:0e9f65e3e2a0 106 motor2->Move(StepperMotor::FWD,nbPas);
Yannick292 4:0e9f65e3e2a0 107 motor1->Move(StepperMotor::FWD,nbPas);
Yannick292 4:0e9f65e3e2a0 108 } else {
Yannick292 4:0e9f65e3e2a0 109 motor2->Move(StepperMotor::BWD,nbPas);
Yannick292 4:0e9f65e3e2a0 110 motor1->Move(StepperMotor::BWD,nbPas);
Yannick292 4:0e9f65e3e2a0 111
Yannick292 4:0e9f65e3e2a0 112 }
julientiron 0:88fd29503679 113 motor1->WaitWhileActive();
julientiron 0:88fd29503679 114 motor2->WaitWhileActive();
julientiron 0:88fd29503679 115 motor1->HardStop();
julientiron 0:88fd29503679 116 motor2->HardStop();
julientiron 0:88fd29503679 117 }
julientiron 0:88fd29503679 118
julientiron 0:88fd29503679 119
julientiron 0:88fd29503679 120 /* Main ----------------------------------------------------------------------*/
julientiron 0:88fd29503679 121
julientiron 0:88fd29503679 122 int main()
julientiron 0:88fd29503679 123 {
julientiron 0:88fd29503679 124 /*----- Initialization. -----*/
julientiron 0:88fd29503679 125
julientiron 0:88fd29503679 126 /* Initializing SPI bus. */
julientiron 0:88fd29503679 127 DevSPI dev_spi(D11, D12, D13);
julientiron 0:88fd29503679 128 myservo.calibrate(0.00095, 90.0); // Calibrate the servo
julientiron 0:88fd29503679 129 parasol.period_ms(10);
julientiron 0:88fd29503679 130 parasol.pulsewidth_ms(1);
julientiron 0:88fd29503679 131 /* Initializing Motor Control Components. */
julientiron 0:88fd29503679 132 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
julientiron 0:88fd29503679 133 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
julientiron 0:88fd29503679 134 if (motor1->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 135 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 136 if (motor2->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 137 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 138 sdaDummy.mode(PullUp);
julientiron 0:88fd29503679 139 sclDummy.mode(PullUp);
julientiron 0:88fd29503679 140 //pwm_ticker.attach(&pwm, 0.1);
julientiron 0:88fd29503679 141 /* Initializing AX-12 motors */
julientiron 0:88fd29503679 142 /* AX12 ax12_1 (D1, D0, 1, 1000000);
julientiron 0:88fd29503679 143 AX12 ax12_2 (D1, D0, 2, 1000000);
julientiron 0:88fd29503679 144 AX12 ax12_3 (D1, D0, 3, 1000000);
julientiron 0:88fd29503679 145 AX12 ax12_4 (D1, D0, 4, 1000000);
julientiron 0:88fd29503679 146 AX12 ax12_5 (D1, D0, 5, 1000000);*/
julientiron 0:88fd29503679 147
julientiron 0:88fd29503679 148 /* Interrupt to start the robot */
julientiron 0:88fd29503679 149 startup.fall(&go);
julientiron 0:88fd29503679 150 /*motor1->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 151 motor2->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 152 motor1->WaitWhileActive();
julientiron 0:88fd29503679 153 motor2->WaitWhileActive();
julientiron 0:88fd29503679 154 motor1->HardStop();
julientiron 0:88fd29503679 155 motor2->HardStop();*/
julientiron 0:88fd29503679 156 //deplacement(10, 0);
julientiron 0:88fd29503679 157 /* rotation(90, 30);
julientiron 0:88fd29503679 158 wait(2);
julientiron 0:88fd29503679 159 rotation(45, 30);
julientiron 0:88fd29503679 160 wait(2);
julientiron 0:88fd29503679 161 rotation(180, 30);
julientiron 0:88fd29503679 162 wait(2);
julientiron 0:88fd29503679 163 rotation(45, 30);*/
julientiron 0:88fd29503679 164 /*motor1->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 165 motor2->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 166 motor1->WaitWhileActive();
julientiron 0:88fd29503679 167 motor2->WaitWhileActive();
julientiron 0:88fd29503679 168 motor1->HardStop();
julientiron 0:88fd29503679 169 motor2->HardStop();
julientiron 0:88fd29503679 170 */
julientiron 0:88fd29503679 171
julientiron 0:88fd29503679 172 while(start) {
julientiron 0:88fd29503679 173 /* Waiting code */
julientiron 0:88fd29503679 174 /* ax12_1.SetSpeed(70);
julientiron 0:88fd29503679 175 ax12_2.SetSpeed(70);
julientiron 0:88fd29503679 176 ax12_3.SetSpeed(70);
julientiron 0:88fd29503679 177 ax12_4.SetSpeed(70);
julientiron 0:88fd29503679 178 ax12_5.SetSpeed(70);*/
julientiron 0:88fd29503679 179 //wait_ms(100);
julientiron 0:88fd29503679 180 }
julientiron 0:88fd29503679 181
julientiron 0:88fd29503679 182 /* Interrupt to stop the robot */
julientiron 0:88fd29503679 183 game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
julientiron 0:88fd29503679 184
julientiron 0:88fd29503679 185 while(end) {
Yannick292 1:5c28b22f3ca0 186
Yannick292 1:5c28b22f3ca0 187 switch(step) {
Yannick292 1:5c28b22f3ca0 188 case 0:
Yannick292 1:5c28b22f3ca0 189 step = 1;
Yannick292 1:5c28b22f3ca0 190 break;
Yannick292 1:5c28b22f3ca0 191
Yannick292 1:5c28b22f3ca0 192 case 1:
Yannick292 1:5c28b22f3ca0 193 //Déplacement 1ers cubes au milieu
Yannick292 4:0e9f65e3e2a0 194 printf("deplacement init \n");
Yannick292 1:5c28b22f3ca0 195 wait_ms(100);
Yannick292 4:0e9f65e3e2a0 196 deplacement(90,1);
Yannick292 4:0e9f65e3e2a0 197
Yannick292 1:5c28b22f3ca0 198 step = 2;
Yannick292 1:5c28b22f3ca0 199 break;
Yannick292 1:5c28b22f3ca0 200
Yannick292 1:5c28b22f3ca0 201 case 2:
Yannick292 4:0e9f65e3e2a0 202 //Mise en place pour pêche
Yannick292 4:0e9f65e3e2a0 203 //motor1->Move(StepperMotor::FWD,4000); //première rotation 90°
Yannick292 4:0e9f65e3e2a0 204 // motor2->Move(StepperMotor::FWD,4000);
Yannick292 4:0e9f65e3e2a0 205 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 206 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 207 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 208 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 209 // printf("Rotation 1 \n");
Yannick292 4:0e9f65e3e2a0 210 //
Yannick292 4:0e9f65e3e2a0 211 //
Yannick292 4:0e9f65e3e2a0 212 // motor1->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
Yannick292 4:0e9f65e3e2a0 213 // motor2->Move(StepperMotor::FWD,6000);
Yannick292 4:0e9f65e3e2a0 214 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 215 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 216 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 217 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 218 //
Yannick292 4:0e9f65e3e2a0 219 // motor1->Move(StepperMotor::FWD,600); //recul pour rotation
Yannick292 4:0e9f65e3e2a0 220 // motor2->Move(StepperMotor::BWD,600);
Yannick292 4:0e9f65e3e2a0 221 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 222 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 223 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 224 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 225 // printf("Recul Rotation 2\n");
Yannick292 4:0e9f65e3e2a0 226 //
Yannick292 4:0e9f65e3e2a0 227 //
Yannick292 4:0e9f65e3e2a0 228 //
Yannick292 4:0e9f65e3e2a0 229 // motor1->Move(StepperMotor::FWD,4000); //deuxième rotation 90° (même sens que première)
Yannick292 4:0e9f65e3e2a0 230 // motor2->Move(StepperMotor::FWD,4000);
Yannick292 4:0e9f65e3e2a0 231 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 232 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 233 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 234 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 235 // printf("Rotation 2 \n");
Yannick292 4:0e9f65e3e2a0 236 //
Yannick292 4:0e9f65e3e2a0 237 //
Yannick292 4:0e9f65e3e2a0 238 // motor1->Move(StepperMotor::BWD,6000); //mise en position
Yannick292 4:0e9f65e3e2a0 239 // motor2->Move(StepperMotor::FWD,6000);
Yannick292 4:0e9f65e3e2a0 240 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 241 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 242 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 243 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 244 // printf("Mise en position\n");
Yannick292 4:0e9f65e3e2a0 245 //
Yannick292 4:0e9f65e3e2a0 246 // motor2->Move(StepperMotor::BWD,4000); //troisième rotation 90°
Yannick292 4:0e9f65e3e2a0 247 // motor1->Move(StepperMotor::BWD,4000);
Yannick292 4:0e9f65e3e2a0 248 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 249 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 250 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 251 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 252 // printf("Rotation 3 \n");
Yannick292 4:0e9f65e3e2a0 253 //
Yannick292 4:0e9f65e3e2a0 254 //
Yannick292 4:0e9f65e3e2a0 255 // motor2->Move(StepperMotor::BWD,500); //Collage mur
Yannick292 4:0e9f65e3e2a0 256 // motor1->Move(StepperMotor::FWD,500);
Yannick292 4:0e9f65e3e2a0 257 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 258 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 259 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 260 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 261 // printf("Collage\n");
Yannick292 4:0e9f65e3e2a0 262 //
Yannick292 4:0e9f65e3e2a0 263 //
Yannick292 4:0e9f65e3e2a0 264 // motor2->Move(StepperMotor::FWD,600); //recul pour rotation
Yannick292 4:0e9f65e3e2a0 265 // motor1->Move(StepperMotor::BWD,600);
Yannick292 4:0e9f65e3e2a0 266 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 267 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 268 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 269 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 270 //
Yannick292 4:0e9f65e3e2a0 271 // motor2->Move(StepperMotor::BWD,4000); //quatrième rotation 90°
Yannick292 4:0e9f65e3e2a0 272 // motor1->Move(StepperMotor::BWD,4000);
Yannick292 4:0e9f65e3e2a0 273 // motor1->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 274 // motor2->WaitWhileActive();
Yannick292 4:0e9f65e3e2a0 275 // motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 276 // motor2->HardStop();
Yannick292 4:0e9f65e3e2a0 277 // printf("Rotation 4 \n");
Yannick292 4:0e9f65e3e2a0 278 printf("deplacement arriere 40 \n");
Yannick292 4:0e9f65e3e2a0 279 deplacement(40,0);
Yannick292 4:0e9f65e3e2a0 280 printf("rotation 90 1 \n");
Yannick292 4:0e9f65e3e2a0 281 rotation(90, 30, 0);
Yannick292 4:0e9f65e3e2a0 282 printf("Rotation 1 \n");
Yannick292 4:0e9f65e3e2a0 283 printf("deplacement coller mur\n");
Yannick292 4:0e9f65e3e2a0 284 deplacement(112,1);
Yannick292 4:0e9f65e3e2a0 285 printf("deplacement arriere\n");
Yannick292 4:0e9f65e3e2a0 286 deplacement(3,0);
Yannick292 4:0e9f65e3e2a0 287 printf("rotation finale\n");
Yannick292 4:0e9f65e3e2a0 288 rotation(90, 30, 1);
Yannick292 4:0e9f65e3e2a0 289 printf("avancement 40\n");
Yannick292 4:0e9f65e3e2a0 290 deplacement(40,1);
Yannick292 1:5c28b22f3ca0 291
Yannick292 4:0e9f65e3e2a0 292 //Mise en position du robot pour pêche
Yannick292 4:0e9f65e3e2a0 293
Yannick292 4:0e9f65e3e2a0 294 step = 3;
Yannick292 4:0e9f65e3e2a0 295 break;
Yannick292 4:0e9f65e3e2a0 296 case 3:
Yannick292 4:0e9f65e3e2a0 297 //déploiement des bras, pêche
Yannick292 1:5c28b22f3ca0 298
Yannick292 4:0e9f65e3e2a0 299 step = 4;
Yannick292 4:0e9f65e3e2a0 300 break;
Yannick292 4:0e9f65e3e2a0 301 case 4:
Yannick292 4:0e9f65e3e2a0 302 //Dépose des poissons dans le filet
Yannick292 1:5c28b22f3ca0 303
Yannick292 4:0e9f65e3e2a0 304 step = 5;
Yannick292 2:bbd254222dd7 305
Yannick292 4:0e9f65e3e2a0 306 case 5:
Yannick292 4:0e9f65e3e2a0 307 break;
Yannick292 1:5c28b22f3ca0 308
Yannick292 1:5c28b22f3ca0 309
Yannick292 4:0e9f65e3e2a0 310 }
julientiron 0:88fd29503679 311 }
julientiron 0:88fd29503679 312
Yannick292 4:0e9f65e3e2a0 313 parasol.write(0.5);
Yannick292 4:0e9f65e3e2a0 314 printf("2");
Yannick292 4:0e9f65e3e2a0 315 motor1->HardStop();
Yannick292 4:0e9f65e3e2a0 316 motor2->HardStop();
julientiron 0:88fd29503679 317 }
julientiron 0:88fd29503679 318
julientiron 0:88fd29503679 319 void pwm()
julientiron 0:88fd29503679 320 {
julientiron 0:88fd29503679 321 }