EB
Dependencies: mbed TCS3200 VL53L0X
main.cpp@0:be724809b234, 2020-01-31 (annotated)
- Committer:
- kirill164
- Date:
- Fri Jan 31 20:43:32 2020 +0000
- Revision:
- 0:be724809b234
Version1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kirill164 | 0:be724809b234 | 1 | #include "mbed.h" |
kirill164 | 0:be724809b234 | 2 | #include "VL53L0X.h" |
kirill164 | 0:be724809b234 | 3 | #include "TCS3200.h" |
kirill164 | 0:be724809b234 | 4 | |
kirill164 | 0:be724809b234 | 5 | ////////////////////// Переменные для драйверов моторов. |
kirill164 | 0:be724809b234 | 6 | DigitalOut M1inA(PC_7); |
kirill164 | 0:be724809b234 | 7 | DigitalOut M1inB(PA_6); |
kirill164 | 0:be724809b234 | 8 | DigitalOut M1pwm(PB_6); |
kirill164 | 0:be724809b234 | 9 | DigitalOut M2inA(PA_12); |
kirill164 | 0:be724809b234 | 10 | DigitalOut M2inB(PA_11); |
kirill164 | 0:be724809b234 | 11 | DigitalOut M2pwm(PB_12); |
kirill164 | 0:be724809b234 | 12 | DigitalOut M12inA(PC_9); |
kirill164 | 0:be724809b234 | 13 | DigitalOut M12inB(PC_8); |
kirill164 | 0:be724809b234 | 14 | DigitalOut M12pwm(PA_7); |
kirill164 | 0:be724809b234 | 15 | |
kirill164 | 0:be724809b234 | 16 | int a = 1; |
kirill164 | 0:be724809b234 | 17 | |
kirill164 | 0:be724809b234 | 18 | |
kirill164 | 0:be724809b234 | 19 | #define ON 1; |
kirill164 | 0:be724809b234 | 20 | #define OFF 0; |
kirill164 | 0:be724809b234 | 21 | #define mPWM a; |
kirill164 | 0:be724809b234 | 22 | ///////////////////////Переменные для дальномеров. |
kirill164 | 0:be724809b234 | 23 | |
kirill164 | 0:be724809b234 | 24 | I2C i2c(D14, D15); |
kirill164 | 0:be724809b234 | 25 | VL53L0X vl_sensor(&i2c); |
kirill164 | 0:be724809b234 | 26 | DigitalOut vl_shutdown(PA_10); |
kirill164 | 0:be724809b234 | 27 | |
kirill164 | 0:be724809b234 | 28 | |
kirill164 | 0:be724809b234 | 29 | |
kirill164 | 0:be724809b234 | 30 | #define SDA_pin D14 |
kirill164 | 0:be724809b234 | 31 | #define SCL_pin D15 |
kirill164 | 0:be724809b234 | 32 | |
kirill164 | 0:be724809b234 | 33 | |
kirill164 | 0:be724809b234 | 34 | ///////////////////////////////////////////////////////// |
kirill164 | 0:be724809b234 | 35 | |
kirill164 | 0:be724809b234 | 36 | Serial pc(SERIAL_TX, SERIAL_RX); |
kirill164 | 0:be724809b234 | 37 | |
kirill164 | 0:be724809b234 | 38 | ////////////////////////// Для дальномеров. |
kirill164 | 0:be724809b234 | 39 | |
kirill164 | 0:be724809b234 | 40 | void VL53L0X() |
kirill164 | 0:be724809b234 | 41 | { |
kirill164 | 0:be724809b234 | 42 | pc.printf("Single VL53L0X\n\n\r"); |
kirill164 | 0:be724809b234 | 43 | |
kirill164 | 0:be724809b234 | 44 | vl_shutdown = 1; //turn VL53L0X on |
kirill164 | 0:be724809b234 | 45 | vl_sensor.init(); |
kirill164 | 0:be724809b234 | 46 | vl_sensor.setModeContinuous(); |
kirill164 | 0:be724809b234 | 47 | vl_sensor.startContinuous(); |
kirill164 | 0:be724809b234 | 48 | |
kirill164 | 0:be724809b234 | 49 | for(int VL = 0; VL <= 5; VL++) |
kirill164 | 0:be724809b234 | 50 | { |
kirill164 | 0:be724809b234 | 51 | pc.printf("%4imm\n\r", vl_sensor.getRangeMillimeters()); |
kirill164 | 0:be724809b234 | 52 | } |
kirill164 | 0:be724809b234 | 53 | } |
kirill164 | 0:be724809b234 | 54 | ////////////////////////////////////Для датчиков цвета. |
kirill164 | 0:be724809b234 | 55 | |
kirill164 | 0:be724809b234 | 56 | TCS3200 color(PH_0, PH_1, PC_2, PC_3, PA_15); |
kirill164 | 0:be724809b234 | 57 | // S0 S1 S2 S3 OUT |
kirill164 | 0:be724809b234 | 58 | |
kirill164 | 0:be724809b234 | 59 | void colors() { |
kirill164 | 0:be724809b234 | 60 | long red, green, blue, clear; |
kirill164 | 0:be724809b234 | 61 | |
kirill164 | 0:be724809b234 | 62 | //Set the scaling factor to 100% |
kirill164 | 0:be724809b234 | 63 | color.SetMode(TCS3200::SCALE_100); |
kirill164 | 0:be724809b234 | 64 | |
kirill164 | 0:be724809b234 | 65 | for(int TC = 0; TC <= 1; TC++) |
kirill164 | 0:be724809b234 | 66 | { |
kirill164 | 0:be724809b234 | 67 | //Read the HIGH pulse width in nS for each color. |
kirill164 | 0:be724809b234 | 68 | //The lower the value, the more of that color is detected |
kirill164 | 0:be724809b234 | 69 | red = color.ReadRed(); |
kirill164 | 0:be724809b234 | 70 | green = color.ReadGreen(); |
kirill164 | 0:be724809b234 | 71 | blue = color.ReadBlue(); |
kirill164 | 0:be724809b234 | 72 | clear = color.ReadClear(); |
kirill164 | 0:be724809b234 | 73 | |
kirill164 | 0:be724809b234 | 74 | printf("RED: %10d \n GREEN: %10d \n BLUE: %10d \n CLEAR: %10d \n_____________\n ", red, green, blue, clear); |
kirill164 | 0:be724809b234 | 75 | |
kirill164 | 0:be724809b234 | 76 | wait(0.1); |
kirill164 | 0:be724809b234 | 77 | } |
kirill164 | 0:be724809b234 | 78 | } |
kirill164 | 0:be724809b234 | 79 | |
kirill164 | 0:be724809b234 | 80 | |
kirill164 | 0:be724809b234 | 81 | ////////////////////////////////////Для драйверов моторов. |
kirill164 | 0:be724809b234 | 82 | void rotate_right() |
kirill164 | 0:be724809b234 | 83 | { |
kirill164 | 0:be724809b234 | 84 | M1inA = ON; |
kirill164 | 0:be724809b234 | 85 | M1inB = OFF; |
kirill164 | 0:be724809b234 | 86 | M1pwm = mPWM; |
kirill164 | 0:be724809b234 | 87 | //motor 1 |
kirill164 | 0:be724809b234 | 88 | M2inA = ON; |
kirill164 | 0:be724809b234 | 89 | M2inB = OFF; |
kirill164 | 0:be724809b234 | 90 | M2pwm = mPWM; |
kirill164 | 0:be724809b234 | 91 | //motor 2 |
kirill164 | 0:be724809b234 | 92 | M12inA = ON; |
kirill164 | 0:be724809b234 | 93 | M12inB = OFF; |
kirill164 | 0:be724809b234 | 94 | M12pwm = mPWM; |
kirill164 | 0:be724809b234 | 95 | //motor 3 |
kirill164 | 0:be724809b234 | 96 | pc.printf("\n rotate right \n"); |
kirill164 | 0:be724809b234 | 97 | } |
kirill164 | 0:be724809b234 | 98 | |
kirill164 | 0:be724809b234 | 99 | void rotate_left() |
kirill164 | 0:be724809b234 | 100 | { |
kirill164 | 0:be724809b234 | 101 | M1inA = OFF; |
kirill164 | 0:be724809b234 | 102 | M1inB = ON; |
kirill164 | 0:be724809b234 | 103 | M1pwm = mPWM; |
kirill164 | 0:be724809b234 | 104 | //motor 1 |
kirill164 | 0:be724809b234 | 105 | M2inA = OFF; |
kirill164 | 0:be724809b234 | 106 | M2inB = ON; |
kirill164 | 0:be724809b234 | 107 | M2pwm = mPWM; |
kirill164 | 0:be724809b234 | 108 | //motor 2 |
kirill164 | 0:be724809b234 | 109 | M12inA = OFF; |
kirill164 | 0:be724809b234 | 110 | M12inB = ON; |
kirill164 | 0:be724809b234 | 111 | M12pwm = mPWM; |
kirill164 | 0:be724809b234 | 112 | //motor 3 |
kirill164 | 0:be724809b234 | 113 | pc.printf("\n rotate left \n"); |
kirill164 | 0:be724809b234 | 114 | } |
kirill164 | 0:be724809b234 | 115 | |
kirill164 | 0:be724809b234 | 116 | void up() |
kirill164 | 0:be724809b234 | 117 | { |
kirill164 | 0:be724809b234 | 118 | M1inA = OFF; |
kirill164 | 0:be724809b234 | 119 | M1inB = ON; |
kirill164 | 0:be724809b234 | 120 | M1pwm = mPWM; |
kirill164 | 0:be724809b234 | 121 | //motor 1 |
kirill164 | 0:be724809b234 | 122 | M2inA = ON; |
kirill164 | 0:be724809b234 | 123 | M2inB = OFF; |
kirill164 | 0:be724809b234 | 124 | M2pwm = mPWM; |
kirill164 | 0:be724809b234 | 125 | //motor 2 |
kirill164 | 0:be724809b234 | 126 | M12inA = OFF; |
kirill164 | 0:be724809b234 | 127 | M12inB = ON; |
kirill164 | 0:be724809b234 | 128 | M12pwm = OFF; |
kirill164 | 0:be724809b234 | 129 | //motor 3 |
kirill164 | 0:be724809b234 | 130 | pc.printf("\n up \n"); |
kirill164 | 0:be724809b234 | 131 | } |
kirill164 | 0:be724809b234 | 132 | |
kirill164 | 0:be724809b234 | 133 | void left() |
kirill164 | 0:be724809b234 | 134 | { |
kirill164 | 0:be724809b234 | 135 | M1inA = ON; |
kirill164 | 0:be724809b234 | 136 | M1inB = OFF; |
kirill164 | 0:be724809b234 | 137 | M1pwm = mPWM; |
kirill164 | 0:be724809b234 | 138 | //motor 1 |
kirill164 | 0:be724809b234 | 139 | M2inA = OFF; |
kirill164 | 0:be724809b234 | 140 | M2inB = ON; |
kirill164 | 0:be724809b234 | 141 | M2pwm = OFF; |
kirill164 | 0:be724809b234 | 142 | //motor 2 |
kirill164 | 0:be724809b234 | 143 | M12inA = OFF; |
kirill164 | 0:be724809b234 | 144 | M12inB = ON; |
kirill164 | 0:be724809b234 | 145 | M12pwm = mPWM; |
kirill164 | 0:be724809b234 | 146 | //motor 3 |
kirill164 | 0:be724809b234 | 147 | pc.printf("\n left \n"); |
kirill164 | 0:be724809b234 | 148 | } |
kirill164 | 0:be724809b234 | 149 | |
kirill164 | 0:be724809b234 | 150 | void right() |
kirill164 | 0:be724809b234 | 151 | { |
kirill164 | 0:be724809b234 | 152 | M1inA = ON; |
kirill164 | 0:be724809b234 | 153 | M1inB = OFF; |
kirill164 | 0:be724809b234 | 154 | M1pwm = OFF; |
kirill164 | 0:be724809b234 | 155 | //motor 1 |
kirill164 | 0:be724809b234 | 156 | M2inA = OFF; |
kirill164 | 0:be724809b234 | 157 | M2inB = ON; |
kirill164 | 0:be724809b234 | 158 | M2pwm = mPWM; |
kirill164 | 0:be724809b234 | 159 | //motor 2 |
kirill164 | 0:be724809b234 | 160 | M12inA = ON; |
kirill164 | 0:be724809b234 | 161 | M12inB = OFF; |
kirill164 | 0:be724809b234 | 162 | M12pwm = mPWM; |
kirill164 | 0:be724809b234 | 163 | //motor 3 |
kirill164 | 0:be724809b234 | 164 | pc.printf("\n right \n"); |
kirill164 | 0:be724809b234 | 165 | } |
kirill164 | 0:be724809b234 | 166 | ///////////////////////////////////////////////////////////// |
kirill164 | 0:be724809b234 | 167 | |
kirill164 | 0:be724809b234 | 168 | int main() |
kirill164 | 0:be724809b234 | 169 | { |
kirill164 | 0:be724809b234 | 170 | while(1) { |
kirill164 | 0:be724809b234 | 171 | rotate_right(); |
kirill164 | 0:be724809b234 | 172 | VL53L0X(); |
kirill164 | 0:be724809b234 | 173 | colors(); |
kirill164 | 0:be724809b234 | 174 | wait(5); |
kirill164 | 0:be724809b234 | 175 | rotate_left(); |
kirill164 | 0:be724809b234 | 176 | VL53L0X(); |
kirill164 | 0:be724809b234 | 177 | colors(); |
kirill164 | 0:be724809b234 | 178 | wait(5); |
kirill164 | 0:be724809b234 | 179 | up(); |
kirill164 | 0:be724809b234 | 180 | VL53L0X(); |
kirill164 | 0:be724809b234 | 181 | colors(); |
kirill164 | 0:be724809b234 | 182 | wait(5); |
kirill164 | 0:be724809b234 | 183 | left(); |
kirill164 | 0:be724809b234 | 184 | VL53L0X(); |
kirill164 | 0:be724809b234 | 185 | colors(); |
kirill164 | 0:be724809b234 | 186 | wait(5); |
kirill164 | 0:be724809b234 | 187 | right(); |
kirill164 | 0:be724809b234 | 188 | VL53L0X(); |
kirill164 | 0:be724809b234 | 189 | colors(); |
kirill164 | 0:be724809b234 | 190 | } |
kirill164 | 0:be724809b234 | 191 | } |