NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2019_usiro_v6

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen

main.cpp

Committer:
skouki
Date:
2019-10-02
Revision:
1:64871263444f
Parent:
0:3ad208cbea5f

File content as of revision 1:64871263444f:

#include"mbed.h"
#include"measuring_wheel.h"
#include"ikarashiMDC.h"
#include"position_controller.h"
#include"omni_wheel.h"
#include"pin_config.h"
#include"SerialMultiByte.h"
#include"PID.h"
#include"IRsensor.h"

#define YPOINT 6400
#define GAP 15
#define MTOU 300

Serial serial(MDCTX,MDCRX,115200);
ikarashiMDC motor[]={
    ikarashiMDC(1,0,SM,&serial),
    ikarashiMDC(1,1,SM,&serial),
    ikarashiMDC(1,2,SM,&serial),
    ikarashiMDC(1,3,SM,&serial)
};
PositionController position_control_1(1000,1000,0.3,0.01,0.3);

OmniWheel omni(4);
SerialMultiByte s(SERIALTX,SERIALRX);
MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
PID pid_spin(0,0,0,0.001);
PID pid_x(0,0,0,0.001);
PID pid_y(0,0,0,0.001);
Serial pc(USBTX,USBRX,115200);

DigitalIn an(USER_BUTTON);
DigitalOut debug_led_0(LED_0);
DigitalOut debug_led_1(LED_2);
DigitalOut debug_led_2(LED_1);

IRsensor IR0(IR_0);
IRsensor IR1(IR_1);

int mode;
int instruction_mode;
double omni_power[4];
double X_power,Y_power;
double spin_power;
float y_point = YPOINT;
int X_,Y_;
double dai_x,dai_low_y;
int gap = GAP;
double ir_distance;
int data_a;
int m_to_u = MTOU;

void set_up()
{
  float theta = PIII / 4;
  omni.wheel[0].setRadian(PIII - theta);
  omni.wheel[1].setRadian(theta);
  omni.wheel[2].setRadian(-theta);
  omni.wheel[3].setRadian(PIII + theta);

  s.setHeaders('A','Z');
  s.startReceive(6);

  IR0.startAveraging(5);
  IR1.startAveraging(5);

}

void mode1()
{
  pid_x.setProcessValue(m.getOutX());
  X_power += pid_x.compute();

  position_control_1.compute(1,m.getOutY());
  Y_power += position_control_1.getVelocityY();

  pid_y.setProcessValue(m.getOutY() - Y_ + 200);
  Y_power += pid_y.compute();

  if(Y_power <= 0.0)Y_power = 0.0;

  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

}

void mode2()
{
  if(data_a)X_power -= 0.3;
  else X_power += 0.3;

  pid_x.setProcessValue(m.getOutX() - X_);
  X_power += pid_x.compute();

  pid_y.setProcessValue(m.getOutY());
  Y_power += pid_y.compute();
  
  if(y_point == 0){
      if(Y_power <= 0.0)Y_power = 0.0;
  }

  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

}

void mode3()
{
  pid_x.setProcessValue(m.getOutX());
  X_power += pid_x.compute();

  pid_y.setProcessValue(Y_ - m.getOutY());
  Y_power -= pid_y.compute();
  
  
  if(Y_power <= 0.0)Y_power = 0.0;

  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

}

void mode4()
{
  pid_x.setProcessValue(m.getOutX());
  X_power += pid_x.compute();

  pid_y.setProcessValue(Y_ - m.getOutY());
  Y_power -= pid_y.compute();
  
  if(Y_power <= 0.0)Y_power = 0.0;


  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

}

void mode5()
{
  if(data_a)X_power -= 0.3;
  else X_power += 0.3;


  pid_x.setProcessValue(m.getOutX() - X_);
  X_power += pid_x.compute();


  pid_y.setProcessValue(Y_ - m.getOutY());
  Y_power -= pid_y.compute();

  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

}

void mode6()
{
  if(data_a)X_power -= 0.3;
  else X_power += 0.3;


  pid_x.setProcessValue(m.getOutX() - X_);
  X_power += pid_x.compute();


  pid_y.setProcessValue(Y_ - m.getOutY());
  Y_power -= pid_y.compute();

  pid_spin.setProcessValue(m.getjyroAngle());
  spin_power = pid_spin.compute();

  if(m.getjyroAngle() <=-10.0)Y_power += 0.5;


}

void to_main()
{
  unsigned char data[1];
  unsigned char getdata[6];
  data[0] = mode;
  s.sendData(data,1);
  s.getData(getdata);
  instruction_mode=getdata[0];

  if(getdata[2]>=(1<<7)){
    getdata[2]-=(1<<7);
    X_=-1*(getdata[1]+(getdata[2]<<8));
  }
  else X_= getdata[1]+(getdata[2]<<8);

  if(getdata[4]>=(1<<7)){
    getdata[4]-=(1<<7);
    Y_=-1*(getdata[3]+(getdata[4]<<8));
  }
  else Y_= getdata[3]+(getdata[4]<<8);
  data_a = getdata[5];
  if(instruction_mode!=0)debug_led_1 = !debug_led_1;
}
int main()
{
  set_up();
    an.mode(PullUp);
  while(true){
    debug_led_0 = !debug_led_0;
    if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){
      debug_led_2 = 1;
    }
    else debug_led_2 = 0;
    X_power = 0.0;
    Y_power = 0.0;
    spin_power = 0.0;
    if(data_a)ir_distance = IR0.get_Averagingdistance();
    else ir_distance = IR1.get_Averagingdistance();

    to_main();

    if(instruction_mode == 1&&mode == 0){

      pid_x.reset();
      pid_x.setTunings(3.0,1.0,0.000001);
      pid_x.setInputLimits(-1000.0,1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(0.0);

      position_control_1.targetXY(1,int(y_point));

      pid_y.reset();
      pid_y.setTunings(7.0,1.0,0.000001);
      pid_y.setInputLimits(-1000.0,1000.0);
      pid_y.setOutputLimits(-1.0,1.0);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(0.0);

      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 1;
    }

    if((m.getOutY() >= (y_point - 260)) && mode == 1){

      pid_x.reset();
      pid_x.setTunings(3.0,1.0,0.000001);
      pid_x.setInputLimits(-1000.0,1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(0.0);

      pid_y.reset();
      pid_y.setTunings(5.0,1.0,0.000001);
      pid_y.setInputLimits(y_point - 150 - 1000.0,y_point - 150 + 1000.0);
      pid_y.setOutputLimits(-1.0,1.0);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(y_point - 200);

      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 2;
    }

    if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){
      dai_x = m.getOutX();
      mode = 0xff;
    }

    if(instruction_mode == 3&&mode == 0xff){
      pid_x.reset();
      pid_x.setTunings(3.0,1.0,0.000001);
      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(dai_x);

      pid_y.reset();
      pid_y.setTunings(10.0,1.0,0.000001);
      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
      pid_y.setOutputLimits(-0.3,0.3);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(m_to_u);

      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 3;
    }

    if(ir_distance>=20&&mode == 3){
      dai_low_y = m.getOutY();

      pid_x.reset();
      pid_x.setTunings(3.0,1.0,0.000001);
      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(dai_x);

 //     pid_y.reset();
//      pid_y.setTunings(10.0,1.0,0.000001);
//      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
//      pid_y.setOutputLimits(-1.0,1.0);
//      pid_y.setBias(0);
//      pid_y.setMode(1);
//      pid_y.setSetPoint(dai_low_y - gap);

      m_to_u = MTOU + 40 ;
      pid_y.reset();
      pid_y.setTunings(10.0,1.0,0.000001);
      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
      pid_y.setOutputLimits(-0.5,0.5);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(m_to_u);
      
      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 4;
    }

    if(instruction_mode == 5&&mode == 4){
      //gap = GAP - 30;

      pid_x.reset();
      pid_x.setTunings(5.0,1.0,0.000001);
      pid_x.setInputLimits(-1000.0,1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(0.0);

//      pid_y.reset();
//      pid_y.setTunings(5.0,1.0,0.000001);
//      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
//      pid_y.setOutputLimits(-1.0,1.0);
//      pid_y.setBias(0);
//      pid_y.setMode(1);
//      pid_y.setSetPoint(dai_low_y - gap);

      pid_y.reset();
      pid_y.setTunings(5.0,1.0,0.000001);
      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
      pid_y.setOutputLimits(-1.0,1.0);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(m_to_u);
      
      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 5;
    }
/*
    if(m.getOutX() <= -1600&&mode == 5){

      //gap = GAP - 50;

      pid_x.reset();
      pid_x.setTunings(1.0,1.0,0.000001);
      pid_x.setInputLimits(-1000.0,1000.0);
      pid_x.setOutputLimits(-1.0,1.0);
      pid_x.setBias(0);
      pid_x.setMode(1);
      pid_x.setSetPoint(0.0);

      pid_y.reset();
      pid_y.setTunings(8.0,1.0,0.000001);
      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
      pid_y.setOutputLimits(-1.0,1.0);
      pid_y.setBias(0);
      pid_y.setMode(1);
      pid_y.setSetPoint(dai_low_y - gap);

      pid_spin.reset();
      pid_spin.setTunings(5.0,1.0,0.000001);
      pid_spin.setInputLimits(-180.0,180.0);
      pid_spin.setOutputLimits(-0.5,0.5);
      pid_spin.setBias(0);
      pid_spin.setMode(1);
      pid_spin.setSetPoint(0.0);

      mode = 6;
    }
    */

    if(an.read()==0)y_point = 0;

    if(mode == 1)mode1();
    if(mode == 2)mode2();
    if(mode == 3)mode3();
    if(mode == 4)mode4();
    if(mode == 5)mode5();
    if(mode == 6)mode6();
    if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;}

    omni.computeXY(Y_power,-X_power,-spin_power);

    for(int i = 0; i < 4; i++){
      omni_power[i] = 0.0;
      omni_power[i] = omni.wheel[i];
      motor[i].setSpeed(omni_power[i]);
    }
    //pc.printf("%.2f,%.2f,%d,%d,%.2f,%.2f\n\r",m.getOutX(),m.getOutY(),X_,Y_,m.getjyroAngle(),ir_distance);


  }
}