linetrace-robot.cpp

/*
* using Nucleo-F303K8
*/
#include "mbed.h"

AnalogIn l(A0),c(A1),r(A2),m(A3);     // line sensor: l(eft), c(enter), m(arker)
float lvu=0, lvu1=0, lvu2=0, lvu3=0;
int sum=0;
float l0=0, l1=0, l2=0, l3=0;
float max0=0, min0=100, max1=0, min1=100, max2=0, min2 = 100, max3=0, min3=100;
//Serial s(USBTX, USBRX);

DigitalIn btn(D13);       // start button

DigitalOut ld_ctrl(D2),
           motorR_in1(D3),
           motorR_in2(D4),
           motorL_in1(D5),
           motorL_in2(D6),
           servo0(D9),
           servo1(D10);

PwmOut   motorR_pwm(D11),
         motorL_pwm(D12);

int state;
double pwm_rate = 0.3;  // motor pwm
int i;             // marker count

void my_pwm(int len, DigitalOut pin) {
  pin = 1;
  wait_us(len);
  pin = 0;
  wait_us(20000 - len);
}

void servo0_init() {
  for (int i=0; i<80; i++)
    my_pwm(570, servo0);
}
void servo0_90() {
  for (int i=0; i<80; i++)
    my_pwm(1300, servo0);
}

void servo1_hold() {
  for (int i=0; i<80; i++)
    my_pwm(500, servo1);
}
void servo1_release() {
  for (int i=0; i<80; i++)
    my_pwm(700, servo1);
}

void move_forward() {
  motorR_in1 = 0;
  motorR_in2 = 1;
  motorL_in1 = 0;
  motorL_in2 = 1;
}
void move_back() {
  motorR_in1 = 1;
  motorR_in2 = 0;
  motorL_in1 = 1;
  motorL_in2 = 0;
}
void move_stop() {
  motorR_in1 = 1;
  motorR_in2 = 1;
  motorL_in1 = 1;
  motorL_in2 = 1;
}

void stop_R() {
  motorR_in1 = 1;
  motorR_in2 = 1;
}
void stop_L() {
  motorL_in1 = 1;
  motorL_in2 = 1;
}

void move_forward_R() {
  motorR_in1 = 0;
  motorR_in2 = 1;
}
void move_back_R() {
  motorR_in1 = 1;
  motorR_in2 = 0;
}

void move_forward_L() {
  motorL_in1 = 0;
  motorL_in2 = 1;
}
void move_back_L() {
  motorL_in1 = 1;
  motorL_in2 = 0;
}


void linetrace() {
  sum = 0;
  if (lvu > 0.50)
    sum -= 1;
  if (lvu2 > 0.50)
    sum += 1;
  if(sum == 0){
    move_forward();
  } else if (sum < 0){
    move_forward_R();
    stop_L();
    if (lvu1 < 0.50)
      move_back_L();
  } else if (sum > 0){
    move_forward_L();
    stop_R();
    if (lvu1 < 0.50){
      move_back_R();
    }
  }
}

void servosystem(){
  if(i==2){
      move_stop();
      move_forward();
      wait(0.2);
      move_forward_R();
      wait(1.5);
      move_stop();
      servo0_init();
      wait_ms(1000);
      move_back_L();
      wait(1.5);
  } if(i==4){
      move_stop();
      move_forward();
      wait(0.2);
      move_forward_L();
      wait(1.5);
      servo0_90();
      wait_ms(1000);
      move_back_R();
      wait (1.5);
  }
}


int main() {
  pwm_rate = 0.3;
  motorR_pwm = pwm_rate;
  motorL_pwm = pwm_rate;
  state = 0;

  i=0;

  while(1) {
    // caliblation
    l0 = l.read();
    l1 = c.read();
    l2 = r.read();
    l3 = m.read();
    max0 = (l0 > max0)?l0:max0;
    min0 = (l0 < min0)?l0:min0;
    max1 = (l1 > max1)?l1:max1;
    min1 = (l1 < min1)?l1:min1;
    max2 = (l2 > max2)?l2:max2;
    min2 = (l2 < min2)?l2:min2;
    max3 = (l3 > max3)?l3:max3;
    min3 = (l3 < min3)?l3:min3;
    lvu =  (l0 - min0)/(max0 - min0);
    lvu1 = (l1 - min1)/(max1 - min1);
    lvu2 = (l2 - min2)/(max2 - min2);
    lvu3 = (l3 - min3)/(max3 - min3);
    wait_ms(1);
    // calibration end
    if (btn)
        break;
  }
  move_forward();
  wait_ms(50);
  move_stop();
  servo0_90();
  while(1) {
    l0 = l.read();
    l1 = c.read();
    l2 = r.read();
    l3 = m.read();
    lvu =  (l0 - min0)/(max0 - min0);
    lvu1 = (l1 - min1)/(max1 - min1);
    lvu2 = (l2 - min2)/(max2 - min2);
    lvu3 = (l3 - min3)/(max3 - min3);

    if (lvu3 > 0.50) {
      move_forward();
      wait_ms(50);
      i++;
      servosystem();
    }

    linetrace();

    wait_ms(1);
  }
}