stilldk
Dependencies: vl53l0x_api SRF05
main.cpp@0:041c1089b6e5, 2022-03-10 (annotated)
- Committer:
- dnikolz
- Date:
- Thu Mar 10 08:40:14 2022 +0000
- Revision:
- 0:041c1089b6e5
idk
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dnikolz | 0:041c1089b6e5 | 1 | #include "mbed.h" |
dnikolz | 0:041c1089b6e5 | 2 | #include "SRF05.h" |
dnikolz | 0:041c1089b6e5 | 3 | #include "Simplifier.h" |
dnikolz | 0:041c1089b6e5 | 4 | #include "Shape.h" |
dnikolz | 0:041c1089b6e5 | 5 | #include "VL53L0XSimplifier.h" |
dnikolz | 0:041c1089b6e5 | 6 | |
dnikolz | 0:041c1089b6e5 | 7 | #define Pin1 PB_0 |
dnikolz | 0:041c1089b6e5 | 8 | #define Pin2 PB_0 |
dnikolz | 0:041c1089b6e5 | 9 | #define Pin_GPIO PB_0 |
dnikolz | 0:041c1089b6e5 | 10 | //DigitalOut myled(LED1); |
dnikolz | 0:041c1089b6e5 | 11 | |
dnikolz | 0:041c1089b6e5 | 12 | PwmOut Drive_pin_A(PA_8); //steering servo PWM output pin D7 |
dnikolz | 0:041c1089b6e5 | 13 | PwmOut Drive_pin_B(PA_9); //Not Used |
dnikolz | 0:041c1089b6e5 | 14 | PwmOut Drive_pin_C(PB_10); //Motor drive PWM output pin D6 |
dnikolz | 0:041c1089b6e5 | 15 | PwmOut Drive_pin_D(PB_4); //Motor drive PWM output pin D5 |
dnikolz | 0:041c1089b6e5 | 16 | |
dnikolz | 0:041c1089b6e5 | 17 | int Turn_right=2; |
dnikolz | 0:041c1089b6e5 | 18 | int Turn_left=0; |
dnikolz | 0:041c1089b6e5 | 19 | int Speed1=10; //Half power |
dnikolz | 0:041c1089b6e5 | 20 | int Speed2=20; //Full power |
dnikolz | 0:041c1089b6e5 | 21 | int PW_Period=20; |
dnikolz | 0:041c1089b6e5 | 22 | |
dnikolz | 0:041c1089b6e5 | 23 | int main() |
dnikolz | 0:041c1089b6e5 | 24 | { |
dnikolz | 0:041c1089b6e5 | 25 | Simplifier::accelerator(); |
dnikolz | 0:041c1089b6e5 | 26 | VL53Simplifier::runVL53(); |
dnikolz | 0:041c1089b6e5 | 27 | // default main |
dnikolz | 0:041c1089b6e5 | 28 | Drive_pin_D.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 29 | Drive_pin_D.pulsewidth_ms(0); |
dnikolz | 0:041c1089b6e5 | 30 | Drive_pin_C.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 31 | Drive_pin_C.pulsewidth_ms(0); //drive motor off |
dnikolz | 0:041c1089b6e5 | 32 | |
dnikolz | 0:041c1089b6e5 | 33 | wait_us(7); |
dnikolz | 0:041c1089b6e5 | 34 | |
dnikolz | 0:041c1089b6e5 | 35 | Drive_pin_A.period_ms(PW_Period); //Steering Servo period |
dnikolz | 0:041c1089b6e5 | 36 | Drive_pin_A.pulsewidth_us(1525); //Steering neutral (includes a 25uS offset) |
dnikolz | 0:041c1089b6e5 | 37 | |
dnikolz | 0:041c1089b6e5 | 38 | wait_us(1); |
dnikolz | 0:041c1089b6e5 | 39 | |
dnikolz | 0:041c1089b6e5 | 40 | Drive_pin_D.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 41 | Drive_pin_D.pulsewidth_ms(0); |
dnikolz | 0:041c1089b6e5 | 42 | Drive_pin_C.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 43 | Drive_pin_C.pulsewidth_ms(Speed1); //drive motor on forward half power |
dnikolz | 0:041c1089b6e5 | 44 | |
dnikolz | 0:041c1089b6e5 | 45 | wait_us(4); |
dnikolz | 0:041c1089b6e5 | 46 | Shape::oval(); |
dnikolz | 0:041c1089b6e5 | 47 | |
dnikolz | 0:041c1089b6e5 | 48 | Drive_pin_A.pulsewidth_us(1275); //Half turn right |
dnikolz | 0:041c1089b6e5 | 49 | |
dnikolz | 0:041c1089b6e5 | 50 | wait_us(7 ); |
dnikolz | 0:041c1089b6e5 | 51 | |
dnikolz | 0:041c1089b6e5 | 52 | Drive_pin_A.pulsewidth_us(1000); // Full steering lock |
dnikolz | 0:041c1089b6e5 | 53 | Drive_pin_C.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 54 | Drive_pin_C.pulsewidth_ms(Speed2); // Full power |
dnikolz | 0:041c1089b6e5 | 55 | |
dnikolz | 0:041c1089b6e5 | 56 | wait_us(5); |
dnikolz | 0:041c1089b6e5 | 57 | |
dnikolz | 0:041c1089b6e5 | 58 | Drive_pin_A.pulsewidth_us(1525); // Neutral steering |
dnikolz | 0:041c1089b6e5 | 59 | |
dnikolz | 0:041c1089b6e5 | 60 | wait_us(1); |
dnikolz | 0:041c1089b6e5 | 61 | |
dnikolz | 0:041c1089b6e5 | 62 | Drive_pin_A.pulsewidth_us(2000); // full steering opposite lock |
dnikolz | 0:041c1089b6e5 | 63 | |
dnikolz | 0:041c1089b6e5 | 64 | wait_us(5.5); |
dnikolz | 0:041c1089b6e5 | 65 | |
dnikolz | 0:041c1089b6e5 | 66 | Drive_pin_A.pulsewidth_us(1525); // Neutral steering |
dnikolz | 0:041c1089b6e5 | 67 | |
dnikolz | 0:041c1089b6e5 | 68 | wait_us(1); |
dnikolz | 0:041c1089b6e5 | 69 | |
dnikolz | 0:041c1089b6e5 | 70 | Drive_pin_D.period_ms(PW_Period); //drive motor reverse |
dnikolz | 0:041c1089b6e5 | 71 | Drive_pin_D.pulsewidth_ms(Speed1); |
dnikolz | 0:041c1089b6e5 | 72 | Drive_pin_C.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 73 | Drive_pin_C.pulsewidth_ms(0); |
dnikolz | 0:041c1089b6e5 | 74 | |
dnikolz | 0:041c1089b6e5 | 75 | wait_us(3); |
dnikolz | 0:041c1089b6e5 | 76 | |
dnikolz | 0:041c1089b6e5 | 77 | Drive_pin_C.period_ms(PW_Period); //drive motor off |
dnikolz | 0:041c1089b6e5 | 78 | Drive_pin_C.pulsewidth_ms(0); |
dnikolz | 0:041c1089b6e5 | 79 | Drive_pin_D.period_ms(PW_Period); |
dnikolz | 0:041c1089b6e5 | 80 | Drive_pin_D.pulsewidth_ms(0); |
dnikolz | 0:041c1089b6e5 | 81 | |
dnikolz | 0:041c1089b6e5 | 82 | } |
dnikolz | 0:041c1089b6e5 | 83 |