Kirill Nik / Mbed 2 deprecated BigBot_v1

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?

UserRevisionLine numberNew 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 }