de
Dependencies: AX12 Servo VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
main.cpp@0:88fd29503679, 2016-05-06 (annotated)
- Committer:
- julientiron
- Date:
- Fri May 06 07:23:13 2016 +0000
- Revision:
- 0:88fd29503679
j;
Who changed what in which revision?
User | Revision | Line number | New 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; |
julientiron | 0:88fd29503679 | 33 | bool tag = true; |
julientiron | 0:88fd29503679 | 34 | int i2cAddres=0x70; // Address of DS1307 is 0x68 (7 bit address) |
julientiron | 0:88fd29503679 | 35 | int i2c8BitAddres= i2cAddres <<1; // Convert to 8bit addressing used by mbed |
julientiron | 0:88fd29503679 | 36 | Servo myservo(D5); |
julientiron | 0:88fd29503679 | 37 | PwmOut parasol(A3); |
julientiron | 0:88fd29503679 | 38 | /* Motor Control Component */ |
julientiron | 0:88fd29503679 | 39 | L6474 *motor1; |
julientiron | 0:88fd29503679 | 40 | L6474 *motor2; |
julientiron | 0:88fd29503679 | 41 | DigitalInOut sdaDummy(D14); |
julientiron | 0:88fd29503679 | 42 | DigitalInOut sclDummy(D15); |
julientiron | 0:88fd29503679 | 43 | /* Distance Sensors Component */ |
julientiron | 0:88fd29503679 | 44 | VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1); |
julientiron | 0:88fd29503679 | 45 | |
julientiron | 0:88fd29503679 | 46 | /* Functions -----------------------------------------------------------------*/ |
julientiron | 0:88fd29503679 | 47 | |
julientiron | 0:88fd29503679 | 48 | void go() |
julientiron | 0:88fd29503679 | 49 | { |
julientiron | 0:88fd29503679 | 50 | start = 0; |
julientiron | 0:88fd29503679 | 51 | } |
julientiron | 0:88fd29503679 | 52 | |
julientiron | 0:88fd29503679 | 53 | void stop() |
julientiron | 0:88fd29503679 | 54 | { |
julientiron | 0:88fd29503679 | 55 | end = 0; |
julientiron | 0:88fd29503679 | 56 | } |
julientiron | 0:88fd29503679 | 57 | |
julientiron | 0:88fd29503679 | 58 | void init_sensor() |
julientiron | 0:88fd29503679 | 59 | { |
julientiron | 0:88fd29503679 | 60 | |
julientiron | 0:88fd29503679 | 61 | } |
julientiron | 0:88fd29503679 | 62 | |
julientiron | 0:88fd29503679 | 63 | void switch_sensor(int number) |
julientiron | 0:88fd29503679 | 64 | { |
julientiron | 0:88fd29503679 | 65 | |
julientiron | 0:88fd29503679 | 66 | } |
julientiron | 0:88fd29503679 | 67 | |
julientiron | 0:88fd29503679 | 68 | void pwm(); |
julientiron | 0:88fd29503679 | 69 | |
julientiron | 0:88fd29503679 | 70 | void deplacement(int distanceEnCm, bool sens) |
julientiron | 0:88fd29503679 | 71 | { |
julientiron | 0:88fd29503679 | 72 | int nbPas = distanceEnCm*166.7; |
julientiron | 0:88fd29503679 | 73 | if(sens) { |
julientiron | 0:88fd29503679 | 74 | motor1->Move(StepperMotor::BWD,nbPas); |
julientiron | 0:88fd29503679 | 75 | motor2->Move(StepperMotor::FWD,nbPas); |
julientiron | 0:88fd29503679 | 76 | } else { |
julientiron | 0:88fd29503679 | 77 | motor1->Move(StepperMotor::FWD,nbPas); |
julientiron | 0:88fd29503679 | 78 | motor2->Move(StepperMotor::BWD,nbPas); |
julientiron | 0:88fd29503679 | 79 | } |
julientiron | 0:88fd29503679 | 80 | motor1->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 81 | motor2->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 82 | motor1->HardStop(); |
julientiron | 0:88fd29503679 | 83 | motor2->HardStop(); |
julientiron | 0:88fd29503679 | 84 | } |
julientiron | 0:88fd29503679 | 85 | |
julientiron | 0:88fd29503679 | 86 | void rotation(int angleEnDegre, int diametre) |
julientiron | 0:88fd29503679 | 87 | { |
julientiron | 0:88fd29503679 | 88 | int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14; |
julientiron | 0:88fd29503679 | 89 | int nbPas = 160*deplacementEnCm; |
julientiron | 0:88fd29503679 | 90 | nbPas /= 990; |
julientiron | 0:88fd29503679 | 91 | |
julientiron | 0:88fd29503679 | 92 | /* |
julientiron | 0:88fd29503679 | 93 | int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14; |
julientiron | 0:88fd29503679 | 94 | int nbPas = 160*deplacementEnCm; |
julientiron | 0:88fd29503679 | 95 | nbPas *= 2;*/ |
julientiron | 0:88fd29503679 | 96 | |
julientiron | 0:88fd29503679 | 97 | printf("%d \n\r", nbPas); |
julientiron | 0:88fd29503679 | 98 | motor2->Move(StepperMotor::FWD,nbPas); |
julientiron | 0:88fd29503679 | 99 | motor1->Move(StepperMotor::FWD,nbPas); |
julientiron | 0:88fd29503679 | 100 | motor1->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 101 | motor2->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 102 | motor1->HardStop(); |
julientiron | 0:88fd29503679 | 103 | motor2->HardStop(); |
julientiron | 0:88fd29503679 | 104 | } |
julientiron | 0:88fd29503679 | 105 | |
julientiron | 0:88fd29503679 | 106 | |
julientiron | 0:88fd29503679 | 107 | /* Main ----------------------------------------------------------------------*/ |
julientiron | 0:88fd29503679 | 108 | |
julientiron | 0:88fd29503679 | 109 | int main() |
julientiron | 0:88fd29503679 | 110 | { |
julientiron | 0:88fd29503679 | 111 | /*----- Initialization. -----*/ |
julientiron | 0:88fd29503679 | 112 | |
julientiron | 0:88fd29503679 | 113 | /* Initializing SPI bus. */ |
julientiron | 0:88fd29503679 | 114 | DevSPI dev_spi(D11, D12, D13); |
julientiron | 0:88fd29503679 | 115 | myservo.calibrate(0.00095, 90.0); // Calibrate the servo |
julientiron | 0:88fd29503679 | 116 | parasol.period_ms(10); |
julientiron | 0:88fd29503679 | 117 | parasol.pulsewidth_ms(1); |
julientiron | 0:88fd29503679 | 118 | /* Initializing Motor Control Components. */ |
julientiron | 0:88fd29503679 | 119 | motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi); |
julientiron | 0:88fd29503679 | 120 | motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); |
julientiron | 0:88fd29503679 | 121 | if (motor1->Init() != COMPONENT_OK) |
julientiron | 0:88fd29503679 | 122 | exit(EXIT_FAILURE); |
julientiron | 0:88fd29503679 | 123 | if (motor2->Init() != COMPONENT_OK) |
julientiron | 0:88fd29503679 | 124 | exit(EXIT_FAILURE); |
julientiron | 0:88fd29503679 | 125 | sdaDummy.mode(PullUp); |
julientiron | 0:88fd29503679 | 126 | sclDummy.mode(PullUp); |
julientiron | 0:88fd29503679 | 127 | //pwm_ticker.attach(&pwm, 0.1); |
julientiron | 0:88fd29503679 | 128 | /* Initializing AX-12 motors */ |
julientiron | 0:88fd29503679 | 129 | /* AX12 ax12_1 (D1, D0, 1, 1000000); |
julientiron | 0:88fd29503679 | 130 | AX12 ax12_2 (D1, D0, 2, 1000000); |
julientiron | 0:88fd29503679 | 131 | AX12 ax12_3 (D1, D0, 3, 1000000); |
julientiron | 0:88fd29503679 | 132 | AX12 ax12_4 (D1, D0, 4, 1000000); |
julientiron | 0:88fd29503679 | 133 | AX12 ax12_5 (D1, D0, 5, 1000000);*/ |
julientiron | 0:88fd29503679 | 134 | |
julientiron | 0:88fd29503679 | 135 | /* Interrupt to start the robot */ |
julientiron | 0:88fd29503679 | 136 | startup.fall(&go); |
julientiron | 0:88fd29503679 | 137 | /*motor1->Move(StepperMotor::BWD,6000); |
julientiron | 0:88fd29503679 | 138 | motor2->Move(StepperMotor::FWD,6000); |
julientiron | 0:88fd29503679 | 139 | motor1->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 140 | motor2->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 141 | motor1->HardStop(); |
julientiron | 0:88fd29503679 | 142 | motor2->HardStop();*/ |
julientiron | 0:88fd29503679 | 143 | //deplacement(10, 0); |
julientiron | 0:88fd29503679 | 144 | /* rotation(90, 30); |
julientiron | 0:88fd29503679 | 145 | wait(2); |
julientiron | 0:88fd29503679 | 146 | rotation(45, 30); |
julientiron | 0:88fd29503679 | 147 | wait(2); |
julientiron | 0:88fd29503679 | 148 | rotation(180, 30); |
julientiron | 0:88fd29503679 | 149 | wait(2); |
julientiron | 0:88fd29503679 | 150 | rotation(45, 30);*/ |
julientiron | 0:88fd29503679 | 151 | /*motor1->Move(StepperMotor::FWD,6000); |
julientiron | 0:88fd29503679 | 152 | motor2->Move(StepperMotor::BWD,6000); |
julientiron | 0:88fd29503679 | 153 | motor1->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 154 | motor2->WaitWhileActive(); |
julientiron | 0:88fd29503679 | 155 | motor1->HardStop(); |
julientiron | 0:88fd29503679 | 156 | motor2->HardStop(); |
julientiron | 0:88fd29503679 | 157 | */ |
julientiron | 0:88fd29503679 | 158 | |
julientiron | 0:88fd29503679 | 159 | while(start) { |
julientiron | 0:88fd29503679 | 160 | /* Waiting code */ |
julientiron | 0:88fd29503679 | 161 | /* ax12_1.SetSpeed(70); |
julientiron | 0:88fd29503679 | 162 | ax12_2.SetSpeed(70); |
julientiron | 0:88fd29503679 | 163 | ax12_3.SetSpeed(70); |
julientiron | 0:88fd29503679 | 164 | ax12_4.SetSpeed(70); |
julientiron | 0:88fd29503679 | 165 | ax12_5.SetSpeed(70);*/ |
julientiron | 0:88fd29503679 | 166 | //wait_ms(100); |
julientiron | 0:88fd29503679 | 167 | } |
julientiron | 0:88fd29503679 | 168 | |
julientiron | 0:88fd29503679 | 169 | /* Interrupt to stop the robot */ |
julientiron | 0:88fd29503679 | 170 | game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe |
julientiron | 0:88fd29503679 | 171 | |
julientiron | 0:88fd29503679 | 172 | while(end) { |
julientiron | 0:88fd29503679 | 173 | //deplacement(60, 1); |
julientiron | 0:88fd29503679 | 174 | //motor1->Run(StepperMotor::FWD); |
julientiron | 0:88fd29503679 | 175 | //motor2->Run(StepperMotor::BWD); |
julientiron | 0:88fd29503679 | 176 | //wait(1); |
julientiron | 0:88fd29503679 | 177 | //motor1->Run(StepperMotor::BWD); |
julientiron | 0:88fd29503679 | 178 | //motor2->Run(StepperMotor::FWD); |
julientiron | 0:88fd29503679 | 179 | wait_ms(100); |
julientiron | 0:88fd29503679 | 180 | motor1->Move(StepperMotor::BWD,12000); |
julientiron | 0:88fd29503679 | 181 | motor2->Move(StepperMotor::FWD,12000); |
julientiron | 0:88fd29503679 | 182 | while((sensor.getDistance()>50) && end) { |
julientiron | 0:88fd29503679 | 183 | //Tant que la distance est superieure on continue d'avancer |
julientiron | 0:88fd29503679 | 184 | wait_ms(20); |
julientiron | 0:88fd29503679 | 185 | printf("1"); |
julientiron | 0:88fd29503679 | 186 | } |
julientiron | 0:88fd29503679 | 187 | } |
julientiron | 0:88fd29503679 | 188 | |
julientiron | 0:88fd29503679 | 189 | parasol.write(0.5); |
julientiron | 0:88fd29503679 | 190 | printf("2"); |
julientiron | 0:88fd29503679 | 191 | motor1->HardStop(); |
julientiron | 0:88fd29503679 | 192 | motor2->HardStop(); |
julientiron | 0:88fd29503679 | 193 | } |
julientiron | 0:88fd29503679 | 194 | |
julientiron | 0:88fd29503679 | 195 | void pwm() |
julientiron | 0:88fd29503679 | 196 | { |
julientiron | 0:88fd29503679 | 197 | } |