makecode-Q-Car/main.ts at master · Wind-stormger/makecode-Q-Car · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
function Motor_control (Right_motor_Positive: number, Right_motor_Negative: number, Left_motor_Positive: number, Left_motor_Negative: number) {
    PCA9685.setPinPulseRange(
    PCA9685.PinNum.Pin0,
    0,
    Right_motor_Positive,
    PCA9685.chipAddress("0x40")
    )
    PCA9685.setPinPulseRange(
    PCA9685.PinNum.Pin1,
    0,
    Right_motor_Negative,
    PCA9685.chipAddress("0x40")
    )
    PCA9685.setPinPulseRange(
    PCA9685.PinNum.Pin3,
    0,
    Left_motor_Positive,
    PCA9685.chipAddress("0x40")
    )
    PCA9685.setPinPulseRange(
    PCA9685.PinNum.Pin2,
    0,
    Left_motor_Negative,
    PCA9685.chipAddress("0x40")
    )
}
function Velocity_measurement () {
    if (input.runningTime() - Last_end_time_2 < 1000) {
        if (pins.digitalReadPin(DigitalPin.P5) == 1 && A_was_pressed == 0) {
            A_was_pressed = 1
        }
        if (pins.digitalReadPin(DigitalPin.P5) == 0 && A_was_pressed == 1) {
            A_was_pressed_times = A_was_pressed_times + 1
            A_was_pressed = 0
        }
        if (pins.digitalReadPin(DigitalPin.P11) == 1 && B_was_pressed == 0) {
            B_was_pressed = 1
        }
        if (pins.digitalReadPin(DigitalPin.P11) == 0 && B_was_pressed == 1) {
            B_was_pressed_times = B_was_pressed_times + 1
            B_was_pressed = 0
        }
    } else if (input.runningTime() - Last_end_time_2 >= 1000) {
        Right_wheel_speed = A_was_pressed_times / 12 * 60
        A_was_pressed_times = 0
        Left_wheel_speed = B_was_pressed_times / 12 * 60
        B_was_pressed_times = 0
        Last_end_time_2 = input.runningTime()
    }
}
function Ultrasonic_ranging () {
    if (input.runningTime() - Last_end_time_1 >= 100) {
        pins.digitalWritePin(DigitalPin.P12, 0)
        pins.digitalWritePin(DigitalPin.P12, 1)
        control.waitMicros(10)
        pins.digitalWritePin(DigitalPin.P12, 0)
        Ultrasonic_time = pins.pulseIn(DigitalPin.P13, PulseValue.High)
        Distance = Ultrasonic_time / 1000000 * (100 * (330.45 + 0.61 * Temperature)) / 2
        Last_end_time_1 = input.runningTime()
    }
}
// Searching for the dark line
function IR_calibration () {
    Line_partol_IR()
    if (Right_IR <= 700 && Right_IR >= 100 && Right_IR < Right_IR_minimum - 200) {
        Right_IR_minimum = Right_IR + 200
    } else if (Left_IR <= 700 && Left_IR >= 100 && Left_IR < Left_IR_minimum - 200) {
        Left_IR_minimum = Left_IR + 200
    } else if (Right_IR < 100) {
        Right_IR_minimum = 300
    } else if (Left_IR < 100) {
        Left_IR_minimum = 300
    }
    serial.writeLine("Right_IR_minimum=" + ("" + Right_IR_minimum) + "|Left_IR_minimum=" + ("" + Left_IR_minimum))
    serial.writeLine("Right_IR_A=" + ("" + Right_IR) + "|Left_IR_A=" + ("" + Left_IR))
    basic.pause(100)
}
function Serial_write () {
    if (input.runningTime() - Last_end_time_3 >= 1000) {
        serial.writeLine("Right wheel speed=" + ("" + Right_wheel_speed) + "|Left wheel speed=" + ("" + Left_wheel_speed))
        serial.writeLine("Distance=" + ("" + Distance) + ("|Temperature=" + ("" + Temperature)))
        serial.writeLine("Right_IR_A=" + ("" + Right_IR) + "|Left_IR_A=" + ("" + Left_IR))
        Last_end_time_3 = input.runningTime()
    }
}
function Line_partol_IR () {
    Right_IR = pins.analogReadPin(AnalogPin.P1)
    Left_IR = pins.analogReadPin(AnalogPin.P2)
}
function Line_patrol_control () {
    if (Right_IR >= Right_IR_minimum && Left_IR >= Left_IR_minimum && Distance > 10) {
        Motor_control(1024, 0, 1024, 0)
    } else if (Right_IR < Right_IR_minimum && Left_IR < Left_IR_minimum && Distance > 10) {
        Motor_control(0, 0, 0, 0)
    } else if (Right_IR >= Right_IR_minimum && Left_IR < Left_IR_minimum && Distance > 10) {
        Motor_control(1024, 0, 0, 0)
    } else if (Right_IR < Right_IR_minimum && Left_IR >= Left_IR_minimum && Distance > 10) {
        Motor_control(0, 0, 1024, 0)
    } else if (Distance <= 10) {
        Motor_control(0, 0, 0, 0)
    }
}
let IR_corrected = 0
let Last_end_time_3 = 0
let Left_IR = 0
let Right_IR = 0
let Distance = 0
let Ultrasonic_time = 0
let Last_end_time_1 = 0
let Left_wheel_speed = 0
let Right_wheel_speed = 0
let B_was_pressed_times = 0
let B_was_pressed = 0
let A_was_pressed_times = 0
let A_was_pressed = 0
let Last_end_time_2 = 0
let Temperature = 0
let Left_IR_minimum = 0
let Right_IR_minimum = 0
PCA9685.init(PCA9685.chipAddress("0x40"), 50)
pins.digitalWritePin(DigitalPin.P14, 0)
Right_IR_minimum = 900
Left_IR_minimum = 900
Temperature = input.temperature()
basic.showIcon(IconNames.SmallDiamond)
basic.showIcon(IconNames.SmallSquare)
basic.showIcon(IconNames.Diamond)
basic.showIcon(IconNames.Square)
basic.clearScreen()
basic.forever(function () {
    if (IR_corrected == 0) {
        basic.showArrow(ArrowNames.East)
    } else if (IR_corrected == 1) {
        basic.showArrow(ArrowNames.West)
    }
    if (input.buttonIsPressed(Button.B) && !(input.buttonIsPressed(Button.A))) {
        pins.digitalWritePin(DigitalPin.P14, 1)
        basic.showIcon(IconNames.Asleep)
        for (let index = 0; index < 20; index++) {
            IR_calibration()
        }
        basic.showIcon(IconNames.Happy)
        pins.digitalWritePin(DigitalPin.P14, 0)
        IR_corrected = 1
    }
    if (input.buttonIsPressed(Button.A) && !(input.buttonIsPressed(Button.B))) {
        basic.showIcon(IconNames.Sword)
        while (true) {
            pins.digitalWritePin(DigitalPin.P14, 1)
            Line_partol_IR()
            Ultrasonic_ranging()
            Line_patrol_control()
            Velocity_measurement()
            Serial_write()
        }
    }
})