Julien Tiron / Mbed 2 deprecated 1-DoorCloser_jor

hh

Dependencies:   VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed

Fork of 1-DoorCloser_jor by Jordan Ml

main.cpp@4:a95f90a0410d, 2016-05-05 (annotated)

Committer:
julientiron
Date:
Thu May 05 20:05:29 2016 +0000
Revision:
4:a95f90a0410d
Parent:
2:f6f12530dbd9 
kk;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julientiron 0:1cb50d31c3b5 1 /**
julientiron 0:1cb50d31c3b5 2 ******************************************************************************
julientiron 0:1cb50d31c3b5 3 * @file main.cpp
julientiron 0:1cb50d31c3b5 4 * @author Julien Tiron, FIP Télécom Bretagne
julientiron 0:1cb50d31c3b5 5 * @version V1.0.0
julientiron 0:1cb50d31c3b5 6 * @date March 23th, 2016
julientiron 0:1cb50d31c3b5 7 * @brief DoorCloser robot main code
julientiron 0:1cb50d31c3b5 8 ******************************************************************************
julientiron 0:1cb50d31c3b5 9 **/
julientiron 0:1cb50d31c3b5 10
julientiron 0:1cb50d31c3b5 11 /* Includes ------------------------------------------------------------------*/
julientiron 0:1cb50d31c3b5 12
julientiron 0:1cb50d31c3b5 13 #include "mbed.h"
julientiron 0:1cb50d31c3b5 14 #include "DevSPI.h"
julientiron 0:1cb50d31c3b5 15 #include "l6474_class.h"
julientiron 0:1cb50d31c3b5 16 #include "DevI2C.h"
julientiron 0:1cb50d31c3b5 17 #include "vl6180x_class.h"
mohabendaoud 2:f6f12530dbd9 18 #include "VL6180x.h"
julientiron 0:1cb50d31c3b5 19
julientiron 0:1cb50d31c3b5 20 /* Definitions ---------------------------------------------------------------*/
julientiron 0:1cb50d31c3b5 21
julientiron 0:1cb50d31c3b5 22 #define VL6180X_ADDRESS 0x29
julientiron 0:1cb50d31c3b5 23
julientiron 0:1cb50d31c3b5 24 /* Variables -----------------------------------------------------------------*/
julientiron 0:1cb50d31c3b5 25
julientiron 0:1cb50d31c3b5 26 /* Start and Stop Component */
mohabendaoud 2:f6f12530dbd9 27
julientiron 0:1cb50d31c3b5 28 InterruptIn startup(PC_1);
julientiron 0:1cb50d31c3b5 29 Ticker game_length;
julientiron 0:1cb50d31c3b5 30 volatile bool start = 1;
julientiron 0:1cb50d31c3b5 31 volatile bool end = 1;
mohabendaoud 2:f6f12530dbd9 32 char data = 0x08 | (char)64;
mohabendaoud 2:f6f12530dbd9 33 bool tag = true;
mohabendaoud 2:f6f12530dbd9 34 int i2cAddres=0x70; // Address of DS1307 is 0x68 (7 bit address)
mohabendaoud 2:f6f12530dbd9 35 int i2c8BitAddres= i2cAddres <<1; // Convert to 8bit addressing used by mbed
julientiron 4:a95f90a0410d 36 volatile unsigned int step = 0;
julientiron 0:1cb50d31c3b5 37 /* Motor Control Component */
julientiron 0:1cb50d31c3b5 38 L6474 *motor1;
julientiron 0:1cb50d31c3b5 39 L6474 *motor2;
julientiron 0:1cb50d31c3b5 40
julientiron 0:1cb50d31c3b5 41 /* Distance Sensors Component */
mohabendaoud 2:f6f12530dbd9 42 //DevI2C *i2c =new DevI2C(D14, D15);
mohabendaoud 1:e18e367432bd 43 /*VL6180X sensor1(i2c);
julientiron 0:1cb50d31c3b5 44 VL6180X sensor2(i2c);
mohabendaoud 1:e18e367432bd 45 VL6180X sensor3(i2c);*/
mohabendaoud 2:f6f12530dbd9 46 I2C i2c(D14, D15);
mohabendaoud 2:f6f12530dbd9 47 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
julientiron 0:1cb50d31c3b5 48
julientiron 0:1cb50d31c3b5 49 /* Functions -----------------------------------------------------------------*/
julientiron 0:1cb50d31c3b5 50
julientiron 0:1cb50d31c3b5 51 void go()
julientiron 0:1cb50d31c3b5 52 {
julientiron 0:1cb50d31c3b5 53 start = 0;
julientiron 0:1cb50d31c3b5 54 }
julientiron 0:1cb50d31c3b5 55
julientiron 0:1cb50d31c3b5 56 void stop()
julientiron 0:1cb50d31c3b5 57 {
julientiron 0:1cb50d31c3b5 58 end = 0;
julientiron 0:1cb50d31c3b5 59 }
julientiron 0:1cb50d31c3b5 60
mohabendaoud 2:f6f12530dbd9 61 void init_sensor()
mohabendaoud 2:f6f12530dbd9 62 {
mohabendaoud 2:f6f12530dbd9 63
julientiron 0:1cb50d31c3b5 64 }
mohabendaoud 2:f6f12530dbd9 65
mohabendaoud 2:f6f12530dbd9 66 void switch_sensor(int number)
mohabendaoud 2:f6f12530dbd9 67 {
mohabendaoud 2:f6f12530dbd9 68
julientiron 0:1cb50d31c3b5 69 }
julientiron 0:1cb50d31c3b5 70
julientiron 4:a95f90a0410d 71 void deplacement(int distanceEnCm)
julientiron 4:a95f90a0410d 72 {
mohabendaoud 2:f6f12530dbd9 73 int nbPas = distanceEnCm*160;
julientiron 4:a95f90a0410d 74 motor1->Move(StepperMotor::BWD,nbPas);
mohabendaoud 2:f6f12530dbd9 75 motor2->Move(StepperMotor::FWD,nbPas);
mohabendaoud 2:f6f12530dbd9 76 motor1->WaitWhileActive();
mohabendaoud 2:f6f12530dbd9 77 motor2->WaitWhileActive();
julientiron 4:a95f90a0410d 78 motor1->HardStop();
julientiron 4:a95f90a0410d 79 motor2->HardStop();
julientiron 4:a95f90a0410d 80 }
julientiron 4:a95f90a0410d 81
julientiron 4:a95f90a0410d 82 void rotationD(int angleEnDegre, int diametre)
julientiron 4:a95f90a0410d 83 {
julientiron 4:a95f90a0410d 84 int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
julientiron 4:a95f90a0410d 85 int nbPas = 160*deplacementEnCm;
julientiron 4:a95f90a0410d 86 nbPas /= 500;
julientiron 4:a95f90a0410d 87
julientiron 4:a95f90a0410d 88 /*
julientiron 4:a95f90a0410d 89 int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
julientiron 4:a95f90a0410d 90 int nbPas = 160*deplacementEnCm;
julientiron 4:a95f90a0410d 91 nbPas *= 2;*/
julientiron 4:a95f90a0410d 92
julientiron 4:a95f90a0410d 93 printf("%d \n\r", nbPas);
julientiron 4:a95f90a0410d 94 motor2->Move(StepperMotor::FWD,nbPas);
julientiron 4:a95f90a0410d 95 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 4:a95f90a0410d 96 motor1->WaitWhileActive();
julientiron 4:a95f90a0410d 97 motor2->WaitWhileActive();
julientiron 4:a95f90a0410d 98 motor1->HardStop();
julientiron 4:a95f90a0410d 99 motor2->HardStop();
julientiron 4:a95f90a0410d 100 }
julientiron 4:a95f90a0410d 101 void rotationG(int angleEnDegre, int diametre)
julientiron 4:a95f90a0410d 102 {
julientiron 4:a95f90a0410d 103 int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
julientiron 4:a95f90a0410d 104 int nbPas = 160*deplacementEnCm;
julientiron 4:a95f90a0410d 105 nbPas /= 500;
julientiron 4:a95f90a0410d 106
julientiron 4:a95f90a0410d 107 /*
julientiron 4:a95f90a0410d 108 int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
julientiron 4:a95f90a0410d 109 int nbPas = 160*deplacementEnCm;
julientiron 4:a95f90a0410d 110 nbPas *= 2;*/
julientiron 4:a95f90a0410d 111
julientiron 4:a95f90a0410d 112 printf("%d \n\r", nbPas);
julientiron 4:a95f90a0410d 113 motor2->Move(StepperMotor::BWD,nbPas);
julientiron 4:a95f90a0410d 114 motor1->Move(StepperMotor::BWD,nbPas);
julientiron 4:a95f90a0410d 115 motor1->WaitWhileActive();
julientiron 4:a95f90a0410d 116 motor2->WaitWhileActive();
julientiron 4:a95f90a0410d 117 motor1->HardStop();
julientiron 4:a95f90a0410d 118 motor2->HardStop();
mohabendaoud 2:f6f12530dbd9 119 }
mohabendaoud 1:e18e367432bd 120
julientiron 0:1cb50d31c3b5 121 /* Main ----------------------------------------------------------------------*/
julientiron 0:1cb50d31c3b5 122
julientiron 0:1cb50d31c3b5 123 int main()
julientiron 0:1cb50d31c3b5 124 {
julientiron 0:1cb50d31c3b5 125 /*----- Initialization. -----*/
julientiron 0:1cb50d31c3b5 126
julientiron 0:1cb50d31c3b5 127 /* Initializing SPI bus. */
julientiron 0:1cb50d31c3b5 128 DevSPI dev_spi(D11, D12, D13);
julientiron 0:1cb50d31c3b5 129
julientiron 0:1cb50d31c3b5 130 /* Initializing Motor Control Components. */
julientiron 0:1cb50d31c3b5 131 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
julientiron 0:1cb50d31c3b5 132 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
julientiron 0:1cb50d31c3b5 133 if (motor1->Init() != COMPONENT_OK)
julientiron 0:1cb50d31c3b5 134 exit(EXIT_FAILURE);
julientiron 0:1cb50d31c3b5 135 if (motor2->Init() != COMPONENT_OK)
julientiron 0:1cb50d31c3b5 136 exit(EXIT_FAILURE);
mohabendaoud 2:f6f12530dbd9 137 int result = i2c.write(i2c8BitAddres, &data, 1 );
mohabendaoud 2:f6f12530dbd9 138 sensor.VL6180xDefautSettings();
julientiron 4:a95f90a0410d 139 wait(1);
julientiron 0:1cb50d31c3b5 140 /* Interrupt to start the robot */
julientiron 0:1cb50d31c3b5 141 startup.fall(&go);
mohabendaoud 2:f6f12530dbd9 142
julientiron 0:1cb50d31c3b5 143
julientiron 0:1cb50d31c3b5 144 while(start) {
julientiron 0:1cb50d31c3b5 145 /* Waiting code */
julientiron 4:a95f90a0410d 146 //deplacement(10);
julientiron 4:a95f90a0410d 147 /*rotation(45, 9);
julientiron 4:a95f90a0410d 148 wait(2);
julientiron 4:a95f90a0410d 149 rotation(90,9);
julientiron 4:a95f90a0410d 150 wait(2);
julientiron 4:a95f90a0410d 151 rotation(45,9);
julientiron 4:a95f90a0410d 152 wait(2);
julientiron 4:a95f90a0410d 153 rotation(180,9);
julientiron 4:a95f90a0410d 154 wait(2);
julientiron 4:a95f90a0410d 155 rotation(45,9);
julientiron 4:a95f90a0410d 156 start = 0;
julientiron 4:a95f90a0410d 157 */
julientiron 4:a95f90a0410d 158 /*
julientiron 4:a95f90a0410d 159 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 4:a95f90a0410d 160 motor2->Move(StepperMotor::BWD,nbPas);
julientiron 4:a95f90a0410d 161 motor1->WaitWhileActive();
julientiron 4:a95f90a0410d 162 motor2->WaitWhileActive();*/
julientiron 0:1cb50d31c3b5 163 }
julientiron 4:a95f90a0410d 164
julientiron 4:a95f90a0410d 165
julientiron 4:a95f90a0410d 166 /* Interrupt to stop the robot */
julientiron 4:a95f90a0410d 167 game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
julientiron 4:a95f90a0410d 168
julientiron 4:a95f90a0410d 169
julientiron 0:1cb50d31c3b5 170 while(end) {
julientiron 0:1cb50d31c3b5 171 /* In-game code */
mohabendaoud 2:f6f12530dbd9 172
mohabendaoud 2:f6f12530dbd9 173
mohabendaoud 2:f6f12530dbd9 174 switch (step) {
mohabendaoud 2:f6f12530dbd9 175
mohabendaoud 2:f6f12530dbd9 176 case 0:
julientiron 4:a95f90a0410d 177
julientiron 4:a95f90a0410d 178 wait(1);
mohabendaoud 2:f6f12530dbd9 179 step = 1;
mohabendaoud 2:f6f12530dbd9 180 break;
mohabendaoud 2:f6f12530dbd9 181
mohabendaoud 2:f6f12530dbd9 182 case 1 :
mohabendaoud 2:f6f12530dbd9 183 /* Avancer du tapis judqu'au mur */
mohabendaoud 2:f6f12530dbd9 184 motor1->Run(StepperMotor::FWD);
mohabendaoud 2:f6f12530dbd9 185 motor2->Run(StepperMotor::BWD);
julientiron 4:a95f90a0410d 186 int dis;
julientiron 4:a95f90a0410d 187 while(((dis = sensor.getDistance())>30)) {
mohabendaoud 2:f6f12530dbd9 188 //Tant que la distance est superieure on continue d'avancer
mohabendaoud 2:f6f12530dbd9 189 wait_ms(20);
mohabendaoud 2:f6f12530dbd9 190 }
julientiron 4:a95f90a0410d 191 printf("%d \n\r", dis);
julientiron 4:a95f90a0410d 192 // motor1->Move(StepperMotor::FWD,1300);
julientiron 4:a95f90a0410d 193 // motor2->Move(StepperMotor::BWD,1300);
julientiron 4:a95f90a0410d 194 // motor1->WaitWhileActive();
julientiron 4:a95f90a0410d 195 // motor2->WaitWhileActive();
mohabendaoud 2:f6f12530dbd9 196 motor1->HardStop();
mohabendaoud 2:f6f12530dbd9 197 motor2->HardStop();
mohabendaoud 2:f6f12530dbd9 198 step = 2 ;
mohabendaoud 2:f6f12530dbd9 199 break;
mohabendaoud 2:f6f12530dbd9 200
mohabendaoud 2:f6f12530dbd9 201 case 2 : /* Angle 90° */
julientiron 4:a95f90a0410d 202 rotationD(90,9); //rotation droite
mohabendaoud 2:f6f12530dbd9 203 motor1->WaitWhileActive();
mohabendaoud 2:f6f12530dbd9 204 motor2->WaitWhileActive();
mohabendaoud 2:f6f12530dbd9 205 step = 3 ;
mohabendaoud 2:f6f12530dbd9 206 break;
mohabendaoud 2:f6f12530dbd9 207
mohabendaoud 2:f6f12530dbd9 208 case 3 : /* Fermeture de 2 portes : 60cm avant + arrêt à 3 cm du mur */
julientiron 4:a95f90a0410d 209 motor1->Move(StepperMotor::FWD,7000); //160pas = 1cm
julientiron 4:a95f90a0410d 210 motor2->Move(StepperMotor::BWD,7000);
mohabendaoud 2:f6f12530dbd9 211 motor1->WaitWhileActive();
mohabendaoud 2:f6f12530dbd9 212 motor2->WaitWhileActive();
julientiron 4:a95f90a0410d 213 rotationG(180,9);
julientiron 4:a95f90a0410d 214 deplacement(20);
julientiron 4:a95f90a0410d 215 rotationD(90,9);
julientiron 4:a95f90a0410d 216 deplacement(20);
julientiron 4:a95f90a0410d 217 rotationD(90,9);
julientiron 4:a95f90a0410d 218 deplacement(40);
julientiron 4:a95f90a0410d 219 motor1->WaitWhileActive();
julientiron 4:a95f90a0410d 220 motor2->WaitWhileActive();
julientiron 4:a95f90a0410d 221
julientiron 4:a95f90a0410d 222 //160 pas = 1 cm
julientiron 4:a95f90a0410d 223 //motor1->Run(StepperMotor::FWD);
julientiron 4:a95f90a0410d 224 //motor2->Run(StepperMotor::BWD);
mohabendaoud 2:f6f12530dbd9 225 while(sensor.getDistance()>30) {
mohabendaoud 2:f6f12530dbd9 226 //Tant que la distance est superieure on continue d'avancer
mohabendaoud 2:f6f12530dbd9 227 wait_ms(20);
mohabendaoud 2:f6f12530dbd9 228 }
mohabendaoud 2:f6f12530dbd9 229 motor1->HardStop();
mohabendaoud 2:f6f12530dbd9 230 motor2->HardStop();
mohabendaoud 2:f6f12530dbd9 231 step = 4;
mohabendaoud 2:f6f12530dbd9 232 break;
mohabendaoud 2:f6f12530dbd9 233
mohabendaoud 2:f6f12530dbd9 234 case 4 :
mohabendaoud 2:f6f12530dbd9 235 // STOP : robot arrêté
mohabendaoud 2:f6f12530dbd9 236 break;
mohabendaoud 2:f6f12530dbd9 237
mohabendaoud 2:f6f12530dbd9 238 }
mohabendaoud 2:f6f12530dbd9 239
julientiron 0:1cb50d31c3b5 240 }
mohabendaoud 2:f6f12530dbd9 241
julientiron 0:1cb50d31c3b5 242 motor1->HardStop();
julientiron 0:1cb50d31c3b5 243 motor2->HardStop();
julientiron 0:1cb50d31c3b5 244
julientiron 0:1cb50d31c3b5 245 motor1->WaitWhileActive();
julientiron 0:1cb50d31c3b5 246 motor2->WaitWhileActive();
julientiron 0:1cb50d31c3b5 247 }