AMAND/driveEndlesslyWithUpdatedNeutral at master · msu-evrl/AMAND · 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
#include <Servo.h>
 Servo myservo;

// Program options
#define MEASURE_TIMING 0    // Measure and display pulse timing
#define SWEEP_STEERING 0    // Sweep the stearing back and forth
#define DRIVE_FORWARD  0    // Drive forward at a given speed
#define DRIVE_BACKWARD 0    // Drive backward at a given speed
#define DRIVE_FWD_BWD  1    // Drive forward then backward

// Pin assignments
int led = 13;         // LED on Teensy 3.0
int btn = 17;         // Button to control start and stop of drive program
int outSignal = 2;   // Output signal to control ESC (electronic speed controller)
int inSignal = 2;    // Input signal from the remote receiver that controls ESC (learn signal pattern)
int steering = 4;    // Output PWM signal to control steering
int pos = 0;

// Variables to control signal timing
unsigned long refTime = 0;  // Starting reference time for the rising or falling edge of signal
unsigned int initTime = 0;  // Initial system time reference to create delay before starting program (reverse requires a delay before starting
unsigned int nLow = 8369;   // Duration of low outSignal for neutral signal
unsigned int nHigh = 1471;  // Duration of high outSignal for neutral signal
unsigned int aLow,aHigh;    // Low and high durations to control ESC (multiple of nLow and nHigh)
bool highState = false;     // Current state of signal (low or high)
unsigned long startDelay = 5000000; // Length of time (microseconds) to wait before string the program (needed for reverse signal)

// Variables to control steering
int wheelPos = 0;           // Wheel position as determined by servo postion (between 0 and 180)
bool wpForward = true;      // Current direction of sweep
unsigned int wpRefTime = 0; // Wheep position reference time
unsigned wpDelay = 10000;      // Number of micro seconds to wait between adjustments

// Variables for a generic wait timer
int waitRefTime = 0;        // Reference timer for a generic wait timer
int systemMode = 0;

// the setup routine runs once when you press reset:
void setup() {
  // Set output pins
  pinMode(led, OUTPUT);
  pinMode(outSignal,OUTPUT);

  // Set the pin to control the servo
  myservo.attach(steering);
  myservo.attach(4);

  // Set the default pulse timings
  aLow = nLow;
  aHigh = nHigh;

  // Start serial communication
  Serial.begin(38400);
}

// Measure pulse timings from receiver
void MeasurePulseTiming() {
  Serial.print(pulseIn(outSignal,HIGH));
  Serial.print(",");
  Serial.println(pulseIn(outSignal,LOW));
}

// Sweep steering back and forth


// Drive forward at speed given (percentage of full power) after driveDelay micro seconds
void DriveForward(int speed, int driveDelay) {


  if (refTime == 0) refTime = micros();
  if (initTime == 0) initTime = micros();
  if (micros() - initTime > driveDelay) {
    aLow = (1-speed/100.0) * nLow;
    aHigh = (1+speed/100.0) * nHigh;
  }

  if (!highState) {
    if (micros() - refTime > aLow) {
      //Serial.print(micros() - refTime);
      //Serial.print(",");
      refTime = micros();
      highState = !highState;
      digitalWrite(outSignal,HIGH);
    }
  }
  else {
    if (micros() - refTime > aHigh) {
      //Serial.println(micros() - refTime);
      refTime = micros();
      highState = !highState;
      digitalWrite(outSignal,LOW);
    }
  }

}

// Drive forward at speed given (percentage of full power) after driveDelay micro seconds
void DriveBackward(int speed, int driveDelay) {
  DriveForward(-speed,driveDelay);
}

// the loop routine runs over and over again forever:
void loop() {

  // Display outSignal lengths (micro seconds)
  //if (MEASURE_TIMING) MeasurePulseTiming();

  // Sweep the steering back and forth
  //if (SWEEP_STEERING) SweepSteering();

  // Drive vehicle forward at a set speed
  //if (DRIVE_FORWARD) DriveForward(5,0);

  // Drive vehicle backward at set speed after 3 second delay
  //if (DRIVE_BACKWARD) DriveBackward(10,3000000);

  // Drive vehicle forward and backwards
  if (DRIVE_FWD_BWD) {
    if (waitRefTime == 0) waitRefTime = micros();
    if (micros() - waitRefTime > 500000) {
      systemMode++;
      waitRefTime = micros();
    }
    switch (systemMode) {
      case 0: DriveForward(6,0); break; Serial.println(systemMode);
      myservo.write(15);
    }
  }
}