robotcar.c

#define dw digitalWrite
#define aw analogWrite

#define maxspeed 1000

struct motor{
  int pins=0;
  bool parity=0;
  int lastspeed=1;
  bool abovezero=1;
};
#define speed 2
#define dir 1
#define dir_ 0

motor left;
motor right;

void init(motor* m){
  pinMode(m->pins+speed,OUTPUT);
  pinMode(m->pins+dir,OUTPUT);
  pinMode(m->pins+dir_,OUTPUT);
  dw(m->pins+dir,m->parity);
  dw(m->pins+dir_, ! m->parity);
  aw(m->pins+speed,1);
}
void newspeed(motor* m,int i){
  if(i==m->lastspeed){return;}
  m->lastspeed=i;
  if(i==0){
    dw(m->pins+speed,LOW);
    delay(10);
    return;
  }
  bool direction=i>0;
  if(direction^m->abovezero){
    newspeed(m,0);
    m->abovezero=direction;
    if(m->parity){
      direction= ! direction;
    }
    dw(m->pins+dir,direction);
    dw(m->pins+dir_, ! direction);
  }
  i=min(abs(i*10),maxspeed);
  aw(m->pins+speed,i);
}

void setup() {
  left.pins=5;
  left.parity=1;
  init(&left);
  right.pins=10;
  right.parity=0;
  init(&right);

}
//
//void loop() {
//  newspeed(&left,50);
//  delay(1000);
//  newspeed(&right,-50);
//  delay(1000);
//  newspeed(&left,-50);
//  newspeed(&right,50);
//  delay(1000);
//  newspeed(&left,0);
//  newspeed(&right,0);
//}

//void loop() {
//  Serial.println(analogRead(1)+String(",")+analogRead(4));
//  delay(100);
//}
void appoch(int* i,int j){
  if(*i<j){
    j-=*i;
    j/=10;
    j+=1;
    *i+=j;
  }else{
    *i=j;
  }
}
void modify(int i,int* a,int* b){
  switch((i>100)+(i>300)+(i>400)){
    case 0: appoch(a,300); break;
    case 1: appoch(a,200); break;
    case 2: appoch(a,-100); break;
    case 3: appoch(a,-300); appoch(b,-100); break;
  }
}
int l=100;
int r=100;
void loop(){
  modify(analogRead(1),&l,&r);
  modify(analogRead(4),&r,&l);
  newspeed(&left,l);
  newspeed(&right,r);
  delay(10);
}