jj
Dependencies: VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
Fork of 1-DoorCloser by
main.cpp@3:cbd7b3a6c722, 2016-04-02 (annotated)
- Committer:
- mohabendaoud
- Date:
- Sat Apr 02 14:48:08 2016 +0000
- Revision:
- 3:cbd7b3a6c722
- Parent:
- 2:f6f12530dbd9
16h48
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 |
mohabendaoud | 2:f6f12530dbd9 | 36 | volatile unsigned int step = 1; |
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 | |
mohabendaoud | 2:f6f12530dbd9 | 71 | void deplacement(int distanceEnCm){ |
mohabendaoud | 2:f6f12530dbd9 | 72 | int nbPas = distanceEnCm*160; |
mohabendaoud | 2:f6f12530dbd9 | 73 | motor1->Move(StepperMotor::FWD,nbPas); |
mohabendaoud | 2:f6f12530dbd9 | 74 | motor2->Move(StepperMotor::BWD,nbPas); |
mohabendaoud | 2:f6f12530dbd9 | 75 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 76 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 77 | } |
mohabendaoud | 2:f6f12530dbd9 | 78 | |
mohabendaoud | 2:f6f12530dbd9 | 79 | void rotation(int angleEnDegre, int diametre){ |
mohabendaoud | 2:f6f12530dbd9 | 80 | int deplacementEnCm = (angleEnDegre/360)*(diametre-2)*3.14; |
mohabendaoud | 2:f6f12530dbd9 | 81 | int nbPas = 160*deplacementEnCm; |
mohabendaoud | 2:f6f12530dbd9 | 82 | motor1->Move(StepperMotor::FWD,nbPas); |
mohabendaoud | 2:f6f12530dbd9 | 83 | motor2->Move(StepperMotor::FWD,nbPas); |
mohabendaoud | 2:f6f12530dbd9 | 84 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 85 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 86 | } |
mohabendaoud | 1:e18e367432bd | 87 | |
julientiron | 0:1cb50d31c3b5 | 88 | /* Main ----------------------------------------------------------------------*/ |
julientiron | 0:1cb50d31c3b5 | 89 | |
julientiron | 0:1cb50d31c3b5 | 90 | int main() |
julientiron | 0:1cb50d31c3b5 | 91 | { |
julientiron | 0:1cb50d31c3b5 | 92 | /*----- Initialization. -----*/ |
julientiron | 0:1cb50d31c3b5 | 93 | |
julientiron | 0:1cb50d31c3b5 | 94 | /* Initializing SPI bus. */ |
julientiron | 0:1cb50d31c3b5 | 95 | DevSPI dev_spi(D11, D12, D13); |
julientiron | 0:1cb50d31c3b5 | 96 | |
julientiron | 0:1cb50d31c3b5 | 97 | /* Initializing Motor Control Components. */ |
julientiron | 0:1cb50d31c3b5 | 98 | motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi); |
julientiron | 0:1cb50d31c3b5 | 99 | motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); |
julientiron | 0:1cb50d31c3b5 | 100 | if (motor1->Init() != COMPONENT_OK) |
julientiron | 0:1cb50d31c3b5 | 101 | exit(EXIT_FAILURE); |
julientiron | 0:1cb50d31c3b5 | 102 | if (motor2->Init() != COMPONENT_OK) |
julientiron | 0:1cb50d31c3b5 | 103 | exit(EXIT_FAILURE); |
mohabendaoud | 2:f6f12530dbd9 | 104 | int result = i2c.write(i2c8BitAddres, &data, 1 ); |
mohabendaoud | 2:f6f12530dbd9 | 105 | sensor.VL6180xDefautSettings(); |
julientiron | 0:1cb50d31c3b5 | 106 | |
julientiron | 0:1cb50d31c3b5 | 107 | /* Interrupt to start the robot */ |
julientiron | 0:1cb50d31c3b5 | 108 | startup.fall(&go); |
mohabendaoud | 2:f6f12530dbd9 | 109 | |
julientiron | 0:1cb50d31c3b5 | 110 | /* Interrupt to stop the robot */ |
julientiron | 0:1cb50d31c3b5 | 111 | game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe |
julientiron | 0:1cb50d31c3b5 | 112 | |
julientiron | 0:1cb50d31c3b5 | 113 | while(start) { |
julientiron | 0:1cb50d31c3b5 | 114 | /* Waiting code */ |
mohabendaoud | 2:f6f12530dbd9 | 115 | |
julientiron | 0:1cb50d31c3b5 | 116 | } |
mohabendaoud | 2:f6f12530dbd9 | 117 | |
julientiron | 0:1cb50d31c3b5 | 118 | while(end) { |
julientiron | 0:1cb50d31c3b5 | 119 | /* In-game code */ |
mohabendaoud | 2:f6f12530dbd9 | 120 | |
mohabendaoud | 2:f6f12530dbd9 | 121 | |
mohabendaoud | 2:f6f12530dbd9 | 122 | switch (step) { |
mohabendaoud | 2:f6f12530dbd9 | 123 | |
mohabendaoud | 2:f6f12530dbd9 | 124 | case 0: |
mohabendaoud | 2:f6f12530dbd9 | 125 | step = 1; |
mohabendaoud | 2:f6f12530dbd9 | 126 | break; |
mohabendaoud | 2:f6f12530dbd9 | 127 | |
mohabendaoud | 2:f6f12530dbd9 | 128 | case 1 : |
mohabendaoud | 2:f6f12530dbd9 | 129 | /* Avancer du tapis judqu'au mur */ |
mohabendaoud | 2:f6f12530dbd9 | 130 | motor1->Run(StepperMotor::FWD); |
mohabendaoud | 2:f6f12530dbd9 | 131 | motor2->Run(StepperMotor::BWD); |
mohabendaoud | 2:f6f12530dbd9 | 132 | while(sensor.getDistance()>30) { |
mohabendaoud | 2:f6f12530dbd9 | 133 | //Tant que la distance est superieure on continue d'avancer |
mohabendaoud | 2:f6f12530dbd9 | 134 | wait_ms(20); |
mohabendaoud | 2:f6f12530dbd9 | 135 | } |
mohabendaoud | 2:f6f12530dbd9 | 136 | motor1->Move(StepperMotor::FWD,1100); |
mohabendaoud | 2:f6f12530dbd9 | 137 | motor2->Move(StepperMotor::BWD,1100); |
mohabendaoud | 2:f6f12530dbd9 | 138 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 139 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 140 | motor1->HardStop(); |
mohabendaoud | 2:f6f12530dbd9 | 141 | motor2->HardStop(); |
mohabendaoud | 2:f6f12530dbd9 | 142 | step = 2 ; |
mohabendaoud | 2:f6f12530dbd9 | 143 | break; |
mohabendaoud | 2:f6f12530dbd9 | 144 | |
mohabendaoud | 2:f6f12530dbd9 | 145 | case 2 : /* Angle 90° */ |
mohabendaoud | 2:f6f12530dbd9 | 146 | motor1->Move(StepperMotor::FWD,2100); |
mohabendaoud | 2:f6f12530dbd9 | 147 | motor2->Move(StepperMotor::FWD,2100); |
mohabendaoud | 2:f6f12530dbd9 | 148 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 149 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 150 | step = 3 ; |
mohabendaoud | 2:f6f12530dbd9 | 151 | break; |
mohabendaoud | 2:f6f12530dbd9 | 152 | |
mohabendaoud | 2:f6f12530dbd9 | 153 | case 3 : /* Fermeture de 2 portes : 60cm avant + arrêt à 3 cm du mur */ |
mohabendaoud | 2:f6f12530dbd9 | 154 | motor1->Move(StepperMotor::FWD,9000); //160pas = 1cm |
mohabendaoud | 2:f6f12530dbd9 | 155 | motor2->Move(StepperMotor::BWD,9000); |
mohabendaoud | 2:f6f12530dbd9 | 156 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 157 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 158 | motor1->Run(StepperMotor::FWD); |
mohabendaoud | 2:f6f12530dbd9 | 159 | motor2->Run(StepperMotor::BWD); |
mohabendaoud | 2:f6f12530dbd9 | 160 | while(sensor.getDistance()>30) { |
mohabendaoud | 2:f6f12530dbd9 | 161 | //Tant que la distance est superieure on continue d'avancer |
mohabendaoud | 2:f6f12530dbd9 | 162 | wait_ms(20); |
mohabendaoud | 2:f6f12530dbd9 | 163 | } |
mohabendaoud | 2:f6f12530dbd9 | 164 | motor1->HardStop(); |
mohabendaoud | 2:f6f12530dbd9 | 165 | motor2->HardStop(); |
mohabendaoud | 2:f6f12530dbd9 | 166 | step = 4; |
mohabendaoud | 2:f6f12530dbd9 | 167 | break; |
mohabendaoud | 2:f6f12530dbd9 | 168 | |
mohabendaoud | 2:f6f12530dbd9 | 169 | case 4 : |
mohabendaoud | 2:f6f12530dbd9 | 170 | // STOP : robot arrêté |
mohabendaoud | 2:f6f12530dbd9 | 171 | break; |
mohabendaoud | 2:f6f12530dbd9 | 172 | |
mohabendaoud | 2:f6f12530dbd9 | 173 | } |
mohabendaoud | 2:f6f12530dbd9 | 174 | |
julientiron | 0:1cb50d31c3b5 | 175 | |
julientiron | 0:1cb50d31c3b5 | 176 | /* Waiting until delay has expired. */ |
mohabendaoud | 2:f6f12530dbd9 | 177 | /*wait_ms(3000); |
mohabendaoud | 2:f6f12530dbd9 | 178 | |
julientiron | 0:1cb50d31c3b5 | 179 | motor1->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 180 | motor2->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 181 | |
julientiron | 0:1cb50d31c3b5 | 182 | motor1->WaitWhileActive(); |
julientiron | 0:1cb50d31c3b5 | 183 | motor2->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 184 | |
julientiron | 0:1cb50d31c3b5 | 185 | /* Requesting to run backward. */ |
mohabendaoud | 2:f6f12530dbd9 | 186 | /*motor1->Run(StepperMotor::FWD); |
julientiron | 0:1cb50d31c3b5 | 187 | motor2->Run(StepperMotor::BWD); |
julientiron | 0:1cb50d31c3b5 | 188 | |
julientiron | 0:1cb50d31c3b5 | 189 | /* Waiting until delay has expired. */ |
mohabendaoud | 2:f6f12530dbd9 | 190 | /*wait_ms(3000); |
mohabendaoud | 2:f6f12530dbd9 | 191 | |
julientiron | 0:1cb50d31c3b5 | 192 | motor1->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 193 | motor2->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 194 | |
julientiron | 0:1cb50d31c3b5 | 195 | motor1->WaitWhileActive(); |
mohabendaoud | 2:f6f12530dbd9 | 196 | motor2->WaitWhileActive();*/ |
julientiron | 0:1cb50d31c3b5 | 197 | } |
mohabendaoud | 2:f6f12530dbd9 | 198 | |
julientiron | 0:1cb50d31c3b5 | 199 | motor1->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 200 | motor2->HardStop(); |
julientiron | 0:1cb50d31c3b5 | 201 | |
julientiron | 0:1cb50d31c3b5 | 202 | motor1->WaitWhileActive(); |
julientiron | 0:1cb50d31c3b5 | 203 | motor2->WaitWhileActive(); |
julientiron | 0:1cb50d31c3b5 | 204 | } |