hh
Dependencies: VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
Fork of 1-DoorCloser_jor by
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?
User | Revision | Line number | New 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 | } |