openinverter forum

Code: Select all

//arduino program written for the nano
//this program is to control the park pawl motor on a nissan leaf transmission


#include <Encoder.h>

// IO 
const int encA = 2;  
const int encB = 3;
const int driveEnable = 4;
const int driveDir = 5;
const int drivePulse = 6;
const int senseIgnition = 7;
const int senseBrake = 8;
const int buttonsRND = 10;
const int buttonP = 11;
const int parkIndicator = 12;

// Other constants
const int posDrive = 0; // in encoder counts
const int posPark = -450; 

const float PulsePerCount = 2.661290323; //encoder counts divided by drive pusles
const int posError = 5;  //number of counts of the encoder that it can vary without causing an error. Also used for the power up routine to find the end of travel.
const int pulseWidth = 1; //in miliseconds for the stepper drive pulse width
const int pulsesPerErrorCheck = 10; 

const int inPark = 1;
const int inDrive = 2;
int pawlState = 0;

Encoder pawlEncoder(encA, encB);

void setup() {
// put your setup code here, to run once:
Serial.begin(115200);

// IO Pin Assignments
pinMode(driveEnable,OUTPUT);
pinMode(driveDir,OUTPUT);
pinMode(drivePulse,OUTPUT);
pinMode(buttonsRND,INPUT_PULLUP);
pinMode(buttonP,INPUT_PULLUP);
pinMode(senseBrake,INPUT);
pinMode(senseIgnition,INPUT);
pinMode(parkIndicator,OUTPUT);


digitalWrite(driveEnable,HIGH); // disable current to the motor

// On power up, assume unknown position. Flash park button until pressed and do homing
while(!(!digitalRead(buttonP) && digitalRead(senseBrake) && digitalRead(senseIgnition))){
  digitalWrite(parkIndicator, HIGH);
  delay(200); // delay for park indicator to flash on and off showing it needs to be pressed
  digitalWrite(parkIndicator, LOW);
  delay(200);
  }

// wait for button release
while(!digitalRead(buttonP)){}

// move to park until a following error is detected (hard stop)
pawlEncoder.write(0);
movePawl(posPark);
delay(2000); // if pressed up against the hard stop, let mechanism relax before zeroing the encoder
pawlEncoder.write(posPark);
pawlState = inPark;

}

void loop() {
// put your main code here, to run repeatedly:

// check if the following is true: RND pressed, brake pressed and ignition on -> go to drive position
if(digitalRead(buttonsRND) && digitalRead(senseBrake) && digitalRead(senseIgnition) && (pawlState == inPark)){
	movePawl(posDrive);
  pawlState = inDrive;
  }   	

// check if the following is true: P pressed, brake pressed and ignition on -> go to park position
if(!digitalRead(buttonP) && digitalRead(senseBrake) && digitalRead(senseIgnition) && (pawlState == inDrive)){
 	movePawl(posPark);
	pawlState = inPark;
	}
   	
// set the state to the correct state while the ignition is on
if(digitalRead(senseIgnition) && (pawlState == inPark)){digitalWrite(parkIndicator, HIGH);}
else{digitalWrite(parkIndicator, LOW);}

}	

int indexA;
int indexB;
long startposition;
int direction;
int errorstate;

void movePawl(int targetposition){

// get the motor ready to run
digitalWrite(driveEnable,LOW);
delay(100); // wait for drive enable to take effect
startposition = pawlEncoder.read();
errorstate = LOW;

// set the motor direction
if (targetposition < startposition){
  direction = -1;
  digitalWrite(driveDir, LOW);
  delay(5); // delay per timing chart for drive on control signal changes
  }
  else {
  direction = 1;
  digitalWrite(driveDir, HIGH);
  delay(5); // delay per timing chart for drive on control signal changes
  }

// move the motor until target position is reached, or position error is detected
indexA = 0;
while (!errorstate && !checkInposition(targetposition,direction)){
  indexB = 0;
  while (indexB < pulsesPerErrorCheck){
    digitalWrite(drivePulse, HIGH);
    delay(pulseWidth);
    digitalWrite(drivePulse, LOW);
    delay(pulseWidth);
    indexB ++;  
    }
  indexA ++;

  // check the current position against the expected position
  if ( abs(startposition + direction*indexA*indexB/PulsePerCount - pawlEncoder.read()) > posError){
    errorstate = HIGH;
    }   
  }

digitalWrite(driveEnable,HIGH); // move done, disable current to motor
}

int checkInposition(int targ, int dir){
  if(dir==1){
    return(pawlEncoder.read() > targ);
    }
  else{
    return(pawlEncoder.read() < targ);
  }
}