//Two-motor position control

//edit these to obtain a good reponse
int posgain=2;   
int velgain=20;  //multiplies vest*interval/1000
long interval=10; //milliseconds between updates
long time;
long tnext;
long tlast;
int mv1dem,mv2dem; //demands sent by PC

int m1f = 13; //motor 1 forwards 
int m1r = 12; //motor 1 reverse 
int m1pwm = 5;
int enc1a = A0;
int enc1b = A1;
volatile int m1pos = 300;
int m1drive = 0;
int m1target = 500;
int m1mov=0;
int m1old=0;
int m1vel=0;

int m2f = 9;  //motor 2 forwards 
int m2r = 10; //motor 2 reverse 
int m2pwm = 11;
int enc2a = A2;
int enc2b = A3;
volatile int m2pos = 300;
int m2drive = 0;
int m2target = 500;
int m2mov=0;
int m2old=0;
int m2vel=0;

int move[16]={0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
int change;
byte bits;
byte oldbits;
uint8_t sendchar[8];

void setup(){
  pinMode(m1f,OUTPUT);
  pinMode(m1r, OUTPUT);
  digitalWrite(m1pwm,HIGH);
  pinMode(m2f,OUTPUT);
  pinMode(m2r, OUTPUT);
  digitalWrite(m2pwm,HIGH);

oldbits=PINC;  
  cli();
  PCICR |= 0b00000010;   // turn on port C interrupts
  PCMSK1 |= 0b00001111;  // enables pins PC0 to PC3,
  sei();
  
  Serial.begin(9600);
  tnext=millis();
}

void loop() {
  time=millis();
  if (time>tnext){
    tnext=time+interval;
    m1vel=(m1pos-m1old);
    m2vel=(m2pos-m2old);
    m1old=m1pos;
    m2old=m2pos;
//Velocities will show rate per interval ms

// motor position control
    m1drive=(m1target-m1pos)*posgain-m1vel*velgain;
    setdrive(m1drive,m1f,m1r,m1pwm);

//motor 2
    m2drive=(m2target-m2pos)*posgain-m2vel*velgain;
    setdrive(m2drive,m2f,m2r,m2pwm);
  } //end of interval
  
  if (Serial.available()>3){
// two bytes each give target positions
    tlast=time;

m1target=Serial.read();
    if(m1target>127){m1target-=256;}  
    //this restores the sign bit
    m1target=256*m1target+Serial.read();
 
    m2target=Serial.read();
    if(m2target>127){m2target-=256;}  
    //this restores the sign bit
    m2target=256*m2target+Serial.read();

// send values to PC

sendchar[0]=m1pos & 0xff;
    sendchar[1]=m1pos >> 8;
    sendchar[2]=m2pos & 0xff;
    sendchar[3]=m2pos >> 8;
    sendchar[4]=m1target & 0xff;
    sendchar[5]=m1target >> 8;
    sendchar[6]=m2target & 0xff;
    sendchar[7]=m2target >> 8;

Serial.write(sendchar,8);    
 
  }  //end of command received
//wakeup if stalled
  if(time-tlast>100){
    tlast=millis();    
    Serial.write(65);
  }
}    //end of loop

ISR(PCINT1_vect){ 
// interrupt routine on encoder change
  bits = PINC;

//for motor 1
  change = ((oldbits & 0b1100) | (bits & 0b11));
  m1mov = move[change]; 
  if(m1mov){  
    m1pos += m1mov;
  }

//for motor 2
  change = ((oldbits & 0b110000) | (bits & 0b1100))>>2;
  m2mov = move[change]; 
  if(m2mov){  
    m2pos += m2mov;
  } 
  oldbits=bits<<2;
}

int lim(int x){
  if(x>255){x=255;}
  if(x<-255){x=-255;}
return x;
}

void setdrive(int m,int f, int r,int p){
  if(m>0){
    digitalWrite(f,HIGH);
    digitalWrite(r,LOW);
  }else{
    digitalWrite(f,LOW);
    digitalWrite(r,HIGH);
  }
  m = 2*lim(abs(m));
  analogWrite(p,m);
}