quad symmetric locomotion

/*
* 1 ---->  right motor ----> e - 8, 22,23-->min85
* 2 ---->  back motor ----> 9, 24, 25-->min85
* 3 ---->  left motor  ----> 10, 26, 27-->min90
* 4 ---->  front motor ----> 11, 28, 29-->min95
*/
#include <ultdist.h>
#include <DCMotor.h>

dcm m1(8,22,23);
dcm m2(9,24,25);
dcm m3(10,26,27);
dcm m4(11,28,29);

int vrmin=0, vbmin=0, vlmin=0, vfmin=0; // calibrate and update
int vr=0, vb=0, vl=0, vf=0,rotf=0, error,kp,vmin=100;
int m1s,m2s,m3s,m4s;
int r1,r2,b1,b2,l1,l2,f1,f2;
/*ultdist u3(33,32);
ultdist u2(35,34);
ultdist u1(37,36);
ultdist u0(39,38);
int d1,d2,d3,d4,i=0;*/

int head,bearing=127,j,i,g,sm,face;
int h1[4],h2[4];
int theta = 0; // for now; have to change later
void read_head()
{
  if (Serial1.available()) 
  {
    head = Serial1.read();
    head = head*1.40625;
    head=360-head;
  }
  for(i=0;i<4;i++)
    h1[i]=head%90 + i*90 ;
  j=0;  
  while(h1[j]!=head)
  {
    j++;
  }
  h2[0]=h1[j];
  if(j==3)
  {
    h2[1]=h1[0];
    h2[2]=h1[1];
    h2[3]=h1[2];
    g=0;
    sm=1;
  }
  else if(j==2)
  {
    h2[1]=h1[3];
    h2[2]=h1[0];
    h2[3]=h1[1];
    g=1;
    sm=2;
  }
  else if(j==1)
  {
    h2[1]=h1[2];
    h2[2]=h1[3];
    h2[3]=h1[0];
    g=2;
    sm=3;
  }
  else if(j==0)
  {
    h2[1]=h1[1];
    h2[2]=h1[2];
    h2[3]=h1[3];
    g=3;
    sm=0;
  }
}
void checkbearing()
{ 
  int q,v;
  for(q=0;q<3;q++)
  {
    if((h2[q]<bearing)&&(bearing<h2[q+1]))                                                                                                                                                                          
      break;
  }
  if(q==3)
  v=0;
  else v=q+1;
  if(q==g)
  {
    if((bearing<=90))
    {
      if((360-h2[q]+bearing)<45){
        //clockwise
        face=q;
        rotf=-1;
        error=abs(360-h2[q]+bearing);
        Serial.print("A");
      }
      else{
        //anti
        face = v;
        rotf=1;
        error=abs(90-(360-h2[q]+bearing));
        //Serial.print("B");
      }
    }
    else if(bearing>=270){
    if((bearing - h2[q])<45){
      //clock
      rotf=-1;
      face=q;
      error=abs(bearing - h2[q]);
      //Serial.print("C");
    }
    
    else{
      //anti
      error=abs(90+bearing-h2[v]);
      rotf=1;
      face = v;
      //Serial.print("D");
    }
    }
  }
  else {
    if(bearing>(h2[q]+45) && bearing<h2[v])
    {
      face = v;
      rotf=1;
      error=abs(h2[v]-bearing);
      //Serial.print("E");
      //anticlock;
    }
    else if(bearing>h2[q] && bearing<h2[v]-45)
    {
      face = q;
      rotf=-1;;
      error=abs(h2[q]-bearing);
      //Serial.print("F");
      //clock;
    }
  }
  /*Serial.print("    ");
  Serial.print(q);
  Serial.print(v);*/
}
void straight()
{
  if(face==3)
  {
    m1.mspeed(255);
    m4.mspeed(255);
    m2.mspeed(-255);
    m3.mspeed(-255);
    
  }
  else if(face==2)
  {
    m2.mspeed(255);
    m1.mspeed(255);
    m3.mspeed(-255);
    m4.mspeed(-255);
    
  }
  else if(face==1)
  {
    m2.mspeed(255);
    m3.mspeed(255);
    m4.mspeed(-255);
    m1.mspeed(-255);
    
  }
  else if(face==0)
  {
    m4.mspeed(255);
    m3.mspeed(255);
    m1.mspeed(-255);
    m2.mspeed(-255);
    
  }
}

void controlhead()
{ 
  kp=((250-vmin)/45);
  if(abs(bearing-h2[face])>4)
  {
    m1.mspeed(rotf*(error*kp + vmin));
    m2.mspeed(rotf*(error*kp + vmin));
    m3.mspeed(rotf*(error*kp + vmin));
    m3.mspeed(rotf*(error*kp + vmin));
  }
  else 
    {
      straight();
    }
}

void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);
  m1.minit();
  m2.minit();
  m3.minit();
  m4.minit();
  /*u1.ultinit();
  u2.ultinit();
  u3.ultinit();
  u4.ultinit();
  pinMode(40,OUTPUT);
  pinMode(41,OUTPUT);
  pinMode(42,OUTPUT);
  pinMode(43,OUTPUT);*/
  //pinMode(2,INPUT);
}

void loop()
{ 
  //m4.mspeed(90);
  //theta++;
  /*if(i%4==0) d1=u1.getdist();
  else if(i%4==1) d2=u2.getdist();
  else if(i%4==2) d3=u3.getdist();
  else d4=u4.getdist();
  i++;*/
  
  read_head(); 
  checkbearing();
  controlhead();
/*Serial.print(face);
  Serial.print("          ");
  Serial.print(error);
  Serial.print("          ");
  Serial.print(rotf);
  Serial.println("          ");*/
  /*Serial.print(h2[0]);
  Serial.print(" ");
  Serial.print(h2[1]);
  Serial.print(" ");
  Serial.print(h2[2]);
  Serial.print(" ");
  Serial.println(h2[3]);*/
  /*read_head(); 
  pidrot(0);
  Serial.println(head1);*/
  /*if(theta==359){
    theta=0;
  }*/
  /*if(digitalRead(2)!=1){
    //Serial.println("HELP");
  }*/
}