File

From ESE205 Wiki
Jump to navigation Jump to search
  1. include <Servo.h>

// Servo Info. // *********************************************************** // Declare Servo Names Servo steering, power; // FSM Info: // *********************************************************** // Declare FSM States

  1. define IDLING 0
  2. define CALC 1
  3. define FORWARD 2
  4. define BACKWARD 3
  5. define RIGHT 4
  6. define LEFT 5
  7. define BRIGHT 6
  8. define BLEFT 7
  9. define KILL 8
  10. define RC 9
  11. define DELAY 10
  12. define TRACK 11
  13. define STOP 12

// FSM Starting State: int state = IDLING; // Timing // *********************************************************** // Timing Variables: unsigned long currentMillis; unsigned long pastMillis; // Timing Interval Variable: unsigned long elapsed; // Assign Duration of Drive Commands: int driveTime = 0; // IR Sensor Chip Info: // *********************************************************** // IR Chip Activator Pin: int activator = 6; // Direction Pins: int northPin = 2; int southPin = 4; int eastPin = 3; int westPin = 5; // Direction Pin Values: boolean northValue; boolean southValue; boolean eastValue; boolean westValue; // Direction Sensors Variables: int north; int south; int east; int west; // Direction of Strongest Signal: int compass; // RC Controller Info: // *********************************************************** // Counting Variable for Smoothing: int x = 0; int y = 0; // Variable for Data Smoothing: int steerSmooth; int throttleSmooth; // Variable for Data Mapping: int steerMap; int throttleMap; // Receiver Pin Assignment: int RcSteer = 8; int RcThrottle = 9; // Default Operation Mode (RC Control On/Off): int RcModeOn = 0; // RC LED Indicator int RcLED = 10; // IR Shoulder Sensors // *********************************************************** // Right Shoulder Sensor int sensorR; int rightIR = 7; // Left Shoulder Sensor int sensorL; int leftIR = 6; // Setup // *********************************************************** // *********************************************************** void setup() { // RC Controller Set-Up: pinMode(RcSteer, INPUT); pinMode(RcThrottle, INPUT); // IR Sensor Set-Up: pinSetup(northPin, southPin, eastPin, westPin, activator); // Servo Pin Set-up // ********************************************************* // Assign PinMode to Pins for Servos pinMode(12, OUTPUT); pinMode(13, OUTPUT); // Assign Servo Pins steering.attach(12); power.attach(13); // LED Pin Setup // ********************************************************* // Assign LED I/O: pinMode(RcLED, OUTPUT); // IR Shoulder Sensors // ********************************************************* pinMode(rightIR, INPUT); pinMode(leftIR, INPUT); // Serial Setup // ********************************************************* // Begin Process Serial.begin(9600); } // Action Loop // *********************************************************** // *********************************************************** void loop() { // FSM // *********************************************************** switch (state) { // Initialization Phase: case IDLING: steerMap = 90; throttleMap = 85; steer(); delay(5000); pastMillis = millis(); state = CALC; break; // Interpret Data and Determine Next Move: case CALC: Serial.println("CALC State "); pastMillis = millis(); senseDirection(northPin, southPin, eastPin, westPin); compass = chooseDirection(northValue, southValue, eastValue, westValue); steerS(); throttleS(); if (throttleSmooth < 1100) { RcToggle(); } else { if (steerSmooth < 1200) { digitalWrite(RcLED, HIGH); state = TRACK; } else { directionInput(compass); } } killSwitch(); break; // Drive Straight Forward: case FORWARD: Serial.println("FORWARD State "); steerMap = 90; throttleMap = 90; state = DELAY; break; // Drive Straight Backwards case BACKWARD: Serial.println("BACKWARD State "); steerMap = 120; throttleMap = 75; state = DELAY; break; // Drive Forward while Turning Right: case RIGHT: Serial.println("RIGHT State "); steerMap = 120; throttleMap = 90; state = DELAY; break; // Drive Forward while Turning Left: case LEFT: Serial.println("LEFT State "); steerMap = 60; throttleMap = 90; state = DELAY; break; // Drive Backwards while Turning Right: case BRIGHT: Serial.println("BRIGHT State "); steerMap = 120; throttleMap = 75; state = DELAY; break; // Drive Backwards While Turning Left: case BLEFT: Serial.println("BLEFT State "); steerMap = 60; throttleMap = 75; state = DELAY; break; // End All Processes: case KILL: steerMap = 90; throttleMap = 85; Serial.println("KILL"); exit(0); break; // Operate as a Remote Controlled Car: case RC: // Add in RC control with collision detection still enabled (Necessary if Collision works outside?) Serial.println("RC Mode"); killSwitch(); steerS(); throttleS(); mapping(); steer(); if (throttleSmooth < 1100) { RcModeOn = 0; steering.write(90); power.write(85); digitalWrite(RcLED, LOW); state = CALC; } break; // Carry out Drive Commands from CALC State (Manage Timing): case DELAY: currentMillis = millis(); steer(); if (throttleSmooth < 1100) { RcToggle(); } elapsed = currentMillis - pastMillis; if (elapsed > driveTime) { Serial.println("END DELAY "); state = CALC; } if (digitalRead(rightIR) == HIGH && digitalRead(leftIR) == HIGH) { state = STOP; } killSwitch(); break; // Line Follower: case TRACK: Serial.print("TRACK "); sensorR = digitalRead(rightIR); sensorL = digitalRead(leftIR); steerS(); throttleS(); if (steerSmooth < 1200) { digitalWrite(RcLED, LOW); state = CALC; } else { if (sensorR == HIGH && sensorL == LOW) { steering.write(120); power.write(85); Serial.print("RIGHT"); } else { if (sensorL == HIGH && sensorR == LOW) { steering.write(60); power.write(85); Serial.print("LEFT"); } else { steering.write(90); power.write(90); Serial.print("FORWARD"); } } } Serial.println(" "); killSwitch(); break; case STOP: Serial.println("STOP"); steerMap = 90; throttleMap = 85; steer(); RcModeOn = 1; state = CALC; break; // Default State (Should Never be Called): default: steerMap = 90; throttleMap = 85; exit(0); break; } } // Functions // *********************************************************** // *********************************************************** // IR Chip Functions // *********************************************************** // Assign IR Chip Pins: void pinSetup(int n, int s, int e, int w, int a) { // Data Inputs: pinMode(n, INPUT); pinMode(s, INPUT); pinMode(e, INPUT); pinMode(w, INPUT); // Activator Pin: pinMode(a, OUTPUT); } // Read Data from Chip: void senseDirection(int sensorPin1, int sensorPin2, int sensorPin3, int sensorPin4) { northValue = digitalRead(sensorPin1); southValue = digitalRead(sensorPin2); eastValue = digitalRead(sensorPin3); westValue = digitalRead(sensorPin4); } // Detect Chip Strongest Direction: int chooseDirection(boolean north, boolean south, boolean east, boolean west) { if (north == 0) { return 1; } if (south == 0) { return 2; } if (east == 0) { return 3; } if (west == 0) { return 4; } } // Radio Control // *********************************************************** // Steering Smoothing: void steerS() { while (x < 5) { x = x + 1; if (steerSmooth = 0) steerSmooth = pulseIn(RcSteer, HIGH); else { steerSmooth = (steerSmooth + pulseIn(RcSteer, HIGH)); } } x = 0; } // Power Smoothing: void throttleS() { while (y < 5) { y = y + 1; if (throttleSmooth = 0) throttleSmooth = pulseIn(RcThrottle, HIGH); else { throttleSmooth = (throttleSmooth + pulseIn(RcThrottle, HIGH)); } } y = 0; } // Map RC Inputs to Servo Outputs: void mapping() { steerMap = map(steerSmooth, 1059, 2200, 0, 180); throttleMap = map(throttleSmooth, 950, 2000, 70, 100); } // Drive Commands: // *********************************************************** // Assign Drive Command to Direction: void directionInput(int Direction) { //Takes/Interprets direction from sensors etc in rest of code. if (Direction == 1) { state = FORWARD; } if (Direction == 2) { state = BACKWARD; } if (Direction == 3) { state = RIGHT; } if (Direction == 4) { state = LEFT; } if (Direction == 5) { state = BRIGHT; } if (Direction == 6) { state = BLEFT; } } // Assign Time for Drive Command to Run: void duration() { state = DELAY; // currentMillis = millis(); // Serial.print("Check Duration "); // Serial.print(currentMillis); // Serial.print(" "); // elapsed = currentMillis - pastMillis; // Serial.print(elapsed); // if ((currentMillis - pastMillis) >= driveTime) { // state = CALC; } // Servo Outputs // *********************************************************** // Write Drive Instructions to Servos: void steer() { steering.write(steerMap); power.write(throttleMap); } // Killswitch // *********************************************************** // Calls Kill State: void killSwitch() { //Set up Signal from RC if (steerSmooth > 1950) { steering.write(90); power.write(85); state = KILL; } } // Drive Mode Toggle void RcToggle() { if (RcModeOn == 0) { RcModeOn = 1; } else { if (RcModeOn == 1) { RcModeOn = 0; } } if (RcModeOn = 1) { digitalWrite(RcLED, HIGH); state = RC; } else { if (RcModeOn = 0) { digitalWrite(RcLED, LOW); state = CALC; } } } // Collision Sensors // *********************************************************** // ADD SAMPLE CODE FOR COLLISION SENSORS // Debugging ETC. // *********************************************************** // Scripted Test of Drive States: void testRobo() { currentMillis = millis(); elapsed = currentMillis - pastMillis; if (elapsed > 0 && elapsed < 2500) { compass = 1; } else { if (elapsed > 2500 && elapsed < 5000) { compass = 2; } else { if (elapsed > 5000 && elapsed < 7500) { compass = 3; } else { if (elapsed > 7500 && elapsed < 10000) { compass = 4; } else { if (elapsed > 12500 && elapsed < 15000) { compass = 5; } else { if (elapsed > 17500 && elapsed < 20000) { compass = 6; } } } } } } } // End Code // ***********************************************************

Retrieved from "http://classes.engineering.wustl.edu/ese205/core/index.php?title=File&oldid=7797"