# Trouble Coding Controls for Autonomous Vehicle

Hello Forum,

I'm working on a project involving an autonomous vehicle and I'm having difficulty writing the code.
The vehicle is powered by a pneumatic piston, controlled by a solenoid. The steering is controlled by a DC brush motor with a potentiometer, for feedback. I thought of using delays to control the solenoid, but, during the delay, the motor would not be monitored so it could drift off course. I've heard about using a state machine to control it or the millis() command. Either way, any advice on how to approach this would be appreciated. Thank you.

dZack

This is the code I'm planning on using to control the motor. I still need to go through it and edit it to do what I want, but the ability to control the motor is there. Also, the goal of the project is for the vehicle to arrive at predetermined destinations. We are using predetermined routes to arrive at these destinations. By route I mean y pushes of the piston and a turn of x degrees, followed by more pushes and/or more turns, depending on the location of the destination. Obviously, a lot of testing is involved to see how the vehicle behaves.

``````/*
Arduino control of a motor

Reads motor position from analog input, compares this to a desired value and computes a control law
Outputs the control signal through PWM along with a separate digital channel that gives the direction of movement

The circuit:
* potentiometer connected to analog pin 0.
Center pin of the potentiometer goes to the analog pin.
side pins of the potentiometer go to +5V and ground
* "P" channel on motor driver connected to digital pin 9
* "D" channel on motor driver connected to digital pin 7

created 26 April 2013
by UCI

*/

// These constants won't change.  They're used to give names
// to the pins used:
const int analogOutPin = 9; // Analog output pin that the "P" channel is attached to (PWM Signal)
const int digitalOutPin = 7; // Digital output pin that the "D" channel is attached to (controls motor Direction)

// PID Gains
const int PGain = -1;
const int DGain = 50;
const int IGain = 0;

// Input and Output Variables
int sensorValue = 0;        // value read from the pot
int outputValue = 0;        // value output to the PWM (analog out)

// Variables needed for Control Law
double potAngle = 0;			// value in degrees
double prevPotAngle = 0;		// previous value read from pot
double desiredPosition = 90;		// desired position (in degrees) we want the motor to go to
double error = 0;				// current error
double motorVelocity = 0;		// first derivative of motorPosition
double errorIntegral = 0;		// used in PID control law
double controlSignal = 0;		// control signal sent to the motor

// Time keeping variables
int count = 0;				// keeps track of time (for switching desired position)
int dt = 1;					// time it takes to execute one loop (in ms)

void setup() {
// initialize serial communications at 9600 bps:
Serial.begin(9600); 		// This allows us to write values to the Serial Monitor
pinMode(digitalOutPin, OUTPUT);	// sets the "D" channel pin to an output
}

void loop() {

// First save the previous value
prevPotAngle = potAngle;
// then save the new value, mapping it to a value in degrees:
potAngle = map(sensorValue, 0, 1023, 0, 360);

// use it to compute error and motor speed
error = potAngle - desiredPosition;
motorVelocity = (potAngle - prevPotAngle)/(dt); // (in deg/ms)
errorIntegral = errorIntegral + error*(dt/1000);

// Calculate the motor control signal using a PID control law
controlSignal = -1*PGain*error - DGain*motorVelocity - IGain*errorIntegral;

// tell the motor which way to spin
if (controlSignal >= 0) {
digitalWrite(digitalOutPin, HIGH);
} else {
digitalWrite(digitalOutPin, LOW);
}

// map it to the range of the analog out:
controlSignal = abs(controlSignal);  // can only ouptut positive values
outputValue = constrain(controlSignal, 0, 255);  // 8-bit resolution

// change the analog out value:
analogWrite(analogOutPin, outputValue);

// print the results to the serial monitor: (UNCOMMENT TO DISPLAY)
//Serial.print("Desired Position = " );
//Serial.print(desiredPosition);
//Serial.print("\t Actual Position = ");
//Serial.println(potAngle);

// wait 2 milliseconds before the next loop
// for the analog-to-digital converter to settle
delay(dt);
count = count + dt;		// increment the counter
if (count >= 1000) {		// every 500 ms, change the desired position of the motor (simulates a square wave input)
count = 0;
if (desiredPosition == 90) {
desiredPosition = 270;
} else {
desiredPosition = 90;
}
}
}
``````

Obviously, a lot of testing is involved to see how the vehicle behaves.

Especially if you have no feedback about how far the vehicle moves on each stroke, or how far it actually turns when turning.

Exactly. But, it doesn't need to travel far, 8 feet at the most.