Reflection
Final Milestone
Modifications | Obstacle avoidance
#include <EnableInterrupt.h> #include <Servo.h> /*Libraries that are required for the code to run. *NOTE: The EnableIntrrupt is not included with Arduino, you will need to download it *manually*/ //Defining constants and variables Servo esc; Servo servo; #define SERIAL_PORT_SPEED 9600 #define RC_NUM_CHANNELS 3 bool hasRun = false; const int trigPin = 10; const int echoPin = 11; long duration; long distance; int runCount = 0; int count = 0; /* * CH1 SWITCH * CH2 STEERING * CH3 THROTTLE */ #define RC_CH1 0 #define RC_CH2 1 #define RC_CH3 2 #define RC_CH1_INPUT A5 #define RC_CH2_INPUT A1 #define RC_CH3_INPUT A0 uint16_t rc_values[RC_NUM_CHANNELS]; uint32_t rc_start[RC_NUM_CHANNELS]; volatile uint16_t rc_shared[RC_NUM_CHANNELS]; void rc_read_values() { //reads values from RC channels noInterrupts(); memcpy(rc_values, (const void *)rc_shared, sizeof(rc_shared)); interrupts(); } void calc_input(uint8_t channel, uint8_t input_pin) { //Calculates the duration of each pulse from the reciever if (digitalRead(input_pin) == HIGH) { rc_start[channel] = micros(); } else { uint16_t rc_compare = (uint16_t)(micros() - rc_start[channel]); rc_shared[channel] = rc_compare; } } void calc_ch1() { calc_input(RC_CH1, RC_CH1_INPUT); } void calc_ch2() { calc_input(RC_CH2, RC_CH2_INPUT); } void calc_ch3() { calc_input(RC_CH3, RC_CH3_INPUT); } void setup() { //Initializes serial port and tells Arduino which pins are connected to what Serial.begin(SERIAL_PORT_SPEED); pinMode(RC_CH1_INPUT, INPUT); pinMode(RC_CH2_INPUT, INPUT); pinMode(RC_CH3_INPUT, INPUT); enableInterrupt(RC_CH1_INPUT, calc_ch1, CHANGE); enableInterrupt(RC_CH2_INPUT, calc_ch2, CHANGE); enableInterrupt(RC_CH3_INPUT, calc_ch3, CHANGE); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); servo.attach(9); esc.attach(13); } void loop() { rc_read_values(); int toggle; int steering; int hover; toggle = rc_values[RC_CH1]; // min 992, max 1984 steering = rc_values[RC_CH2]; // min 996, mid 1488, max 1988 hover = rc_values[RC_CH3]; // min 996 max 2000 if (steering == 0){ steering = 92; } steering = map(steering, 996, 1988, 47, 137); hover = map(hover, 996, 2000, 1000, 2000); //Outputs data to the serial monitor for easier debugging Serial.print("Switch: "); Serial.print(toggle); Serial.print("\t"); Serial.print("Servo :"); Serial.print(steering); Serial.print("\t"); Serial.print("Throttle: "); Serial.print(hover); Serial.print("\t"); Serial.print("Distance :"); Serial.print(distance); Serial.println("\t"); Serial.print("runCount: "); Serial.print(runCount); //Serial.print("\t"); Serial.print("Distance :"); Serial.print(distance); //Serial.println("\t"); Serial.print("HasTurned: "); Serial.print(hasRun); Serial.println("\t"); if (toggle <= 1500) { //Obstacle avoidance OFF esc.write(hover); servo.write(steering); } else { //Obstacle avoidance ON digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = duration * 0.034/2; if (distance <= 60) { runCount = 0; count = count + 1; if (count > 5 && hasRun == false) { count = 0; Serial.println("TURN"); hasRun = true; servo.write(47); esc.write(1000); delay(1500); esc.write(1250); delay(3000); } else { esc.write(hover); servo.write (steering); } } else if (runCount > 10) { hasRun = false; esc.write(hover); servo.write (steering); } else { runCount = runCount + 1; esc.write(hover); servo.write (steering); } } }