Gear selection control

I've had a go at controlling the gear selection motor and it seems to broadly work now. I've done a quick video to explain. 

This is a better video with the voltage now running at 12v, motor takes around 5A on a move and has a bit of a kick to it.

I've had some requests for the Arduino code I used, this was based on some stuff I found on the web, (sorry but i can't remember the source) and my rather limited abilities with Arduino. I then connected this to Makerplot which is a serial port plotter capable of sending and reading some simple commands. It works in a rather odd way as the GUI is a macro you load up, but it's a great program once you're used to it.
So the Arduino code is shown below..
This is a link to the Makerplot software - http://www.makerplot.com/
and the is the macro for the Makerplot as shown on the vid.


This is the photo I found which shows the connection onto the Smart gear selection motor

also a handy schematic of the pins and encoders

----------------------------------- Code to drive motor as shown in the youtube video ------------------
please note, this is note code written spacifically for this project so there is some stuff in here which is not really needed... but it worked for me. You'll probably need to tweak around with the PID settings.


/*
   This program uses an Arduino for a closed-loop control of a DC-motor.
   Motor motion is detected by a quadrature encoder.
   Two inputs named STEP and DIR allow changing the target position.
   Serial port prints current position and target position every second.
   Serial input can be used to feed a new location for the servo (no CR LF).
 
   Pins used:
   Digital inputs 2 & 8 are connected to the two encoder signals (AB).
   Digital input 3 is the STEP input.
   Analog input 0 is the DIR input.
   Digital outputs 5 & 6 control the PWM outputs for the motor (I am using half L298 here).


   Please note PID gains kp, ki, kd need to be tuned to each different setup.
*/
#include <SPI.h>
#include <PID_v1.h>
#define encoder0PinA  2 // PD2;
#define encoder0PinB  8  // PB0;
#define  IS_1  0
#define  IS_2  1
#define  IN_1  3
#define  IN_2  11
#define  INH_1 12
#define  INH_2 13

double kp=300,ki=0,kd=0.5;
double input=80, output=0, setpoint=180;
PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);

volatile long encoder0Pos = 0;
long previousMillis = 0;        // will store last time LED was updated
long target1=0;  // destination location at any moment
int countStep;

//for motor control ramps 1.4
bool newStep = false;
bool oldStep = false;
bool dir = false;


void setup() {
  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);

  pinMode(IN_1,OUTPUT);
  pinMode(IN_2,OUTPUT);
  pinMode(INH_1,OUTPUT);
  pinMode(INH_2,OUTPUT);

  reset_ports();

  digitalWrite(INH_1,1);
  digitalWrite(INH_2,1);

  attachInterrupt(0, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  //attachInterrupt(1, countStep, RISING);  //on pin 3

  Serial.begin (115200);

  //Setup the pid
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255,255);
}

void reset_ports()
{
  digitalWrite(INH_1,0);
  digitalWrite(INH_2,0);
}

void loop(){
    input = encoder0Pos;
    setpoint=target1;
    myPID.Compute();
    pwmOut(output);
   // print encoder and target every second throgh the serial port
    if(millis() > previousMillis+200 )  {Serial.print(encoder0Pos); Serial.print(","); Serial.println(target1);     previousMillis=millis(); //}
    // interpret received data as an integer (no CR LR)

   // if(Serial.available()) {
    Serial.print("!O H1Read=");
    Serial.println(encoder0Pos);
    Serial.println("!READ(H1Set)");
    target1=Serial.parseInt();
    }
}

void pwmOut(int out) {
   if(out<0) { analogWrite(IN_1,0); analogWrite(IN_2,abs(out)); }
   else { analogWrite(IN_2,0); analogWrite(IN_1,abs(out)); }
  }


void doEncoderMotor0(){
  if (((PIND&B0000100)>>2) == HIGH) {   // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PB0
    if ((PINB&B0000001) == LOW) {  // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
      encoder0Pos-- ;         // CCW
    }
    else {
      encoder0Pos++ ;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  {
    if ((PINB&B0000001) == LOW) {   // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
                                              // encoder is turning
      encoder0Pos++ ;          // CW
    }
    else {
      encoder0Pos-- ;          // CCW
    }
  }
}