Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * Speed should be a floating point value [-1,1].
- * i.e. 0.5 means 50% speed in the forward direction, -0.25 would be 25% speed backwards
- * Time is a value in milliseconds (1000 milliseconds in 1 second)
- */
- #define E1 6
- #define E2 5
- #define M1 8
- #define M2 7
- #define trigPin 2
- #define echoPin 3
- void rover_right_track(float s) {
- if (s > 1.0) s = 1.0;
- if ( s < -1.0) s = -1.0;
- int spd = 255 * s;
- analogWrite(E1, abs(spd));
- digitalWrite(M1, (spd >= 0 ? LOW: HIGH));
- }
- void rover_left_track(float s) {
- if (s > 1.0) s = 1.0;
- if ( s < -1.0) s = -1.0;
- int spd = 255 * s;
- analogWrite(E2, abs(spd));
- digitalWrite(M2, (spd >= 0 ? LOW: HIGH));
- }
- void rover_drive(float s, int time) {
- if (s > 1.0) s = 1.0;
- if ( s < -1.0) s = -1.0;
- int spd = 255 * s;
- rover_right_track(spd);
- rover_left_track(spd);
- delay(time);
- rover_stop();
- }
- void rover_rotate_clockwise(float s, int time) {
- if (s > 1.0) s = 1.0;
- if ( s < -1.0) s = -1.0;
- int spd = 255 * s;
- rover_right_track(-spd);
- rover_left_track(spd);
- delay(time);
- rover_stop();
- }
- void rover_rotate_counter_clockwise(float s, int time) {
- if (s > 1.0) s = 1.0;
- if ( s < -1.0) s = -1.0;
- int spd = 255 * s;
- rover_right_track(spd);
- rover_left_track(-spd);
- delay(time);
- rover_stop();
- }
- void rover_stop() {
- rover_right_track(0);
- rover_left_track(0);
- }
- // Call this to end the mission
- void mission_end() {
- rover_stop();
- exit(0);
- }
- void setup_hcsr04() {
- pinMode(trigPin, OUTPUT);
- pinMode(echoPin, INPUT);
- }
- float readDistance() {
- // Clear the trigPin by setting it LOW:
- digitalWrite(trigPin, LOW);
- delayMicroseconds(5);
- // Trigger the sensor by setting the trigPin high for 10 microseconds:
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(trigPin, LOW);
- // Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
- float duration = pulseIn(echoPin, HIGH);
- // Calculate the distance:
- float distance = duration*0.034/2;
- return distance;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement