File content as of revision 1:697097e548f9:

#include"mbed.h"
#include"SerialMultiByte.h"
#include"pin_config.h"
#include"IRsensor.h"
#include"QEI.h"
#include"PID.h"
#include"ikarashiMDC.h"

#define POINT 22380

SerialMultiByte sita_s(S_SERIALTX,S_SERIALRX);
SerialMultiByte ue_s(U_SERIALTX,U_SERIALRX);
DigitalIn start(STARTPIN);
Serial pc(USBTX,USBRX,115200);

DigitalOut debug_led_0(LED0);
DigitalOut debug_led_1(LED1);
DigitalOut debug_led_2(LED2);
DigitalIn mode_r_b(MODE_R_B);
PID pid(5.0,3.0,0.000001,0.001);

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)
};

QEI wheel_2(QEI_1_1,QEI_1_2, NC, 500,QEI::X4_ENCODING);
QEI wheel_1(QEI_3_1,QEI_3_2, NC, 500,QEI::X4_ENCODING);
/*
IRsensor IR0(IR_0);
IRsensor IR1(IR_1);
IRsensor IR2(IR_2);
*/
int mode=0;
int u_mode=0,s_mode=0,m_mode=0;
unsigned char itidata[4];
int X_,Y_;
int data_a;
double power[4];
bool end_flag = true;
bool down_flag = false;
bool debugmode = false;
int height_point = POINT;
double plus_power;
int field;

void to_sita(){
  unsigned char data[6];
  unsigned char getdata[1];
  data[0] = mode;
   for(int i=0;i<4;i++){
     data[i+1] = itidata[i];
  }
  data[5] = field;
  sita_s.sendData(data,6);
  sita_s.getData(getdata);
  s_mode = getdata[0];
}
/*
void ir_checker(){
  float ir_0 = IR0.get_Averagingdistance();
  float ir_1 = IR1.get_Averagingdistance();
  float ir_2 = IR2.get_Averagingdistance();
  pc.printf("%.2f\\%.2f\\%.2f\\",ir_0,ir_1,ir_2);

  data_a = 2;
  if(ir_0 <= 40.0) {
    data_a = 1;
    return;
  }
  if(ir_2 <= 40.0){
    data_a = 3;
    return;
  }
  data_a = 0;
}
*/
void to_ue(){
  unsigned char data[2];
  unsigned char getdata[5];
  data[0] = mode;
  data[1] = field;
  ue_s.sendData(data,2);
  ue_s.getData(getdata);
  u_mode=getdata[0];
  for(int i=0;i<4;i++){
     itidata[i] = getdata[i+1];
  }
}

void syoukou()
{  

  if(debugmode)return;
  if(end_flag&&!down_flag){
    if(height_point >= wheel_1.getPulses())power[0] = 0.8;
    else power[0] = 0.0;
    if(height_point >= wheel_2.getPulses()){
      pid.setProcessValue(wheel_1.getPulses() - wheel_2.getPulses());
      plus_power = pid.compute();
      power[1] = 0.8 - plus_power;
    }
    else power[1] = 0.0;
    if((height_point <= wheel_1.getPulses())&&(height_point <= wheel_2.getPulses()))down_flag = true;
  }
  if(!end_flag){
    power[0] += 0.0;
    power[1] += 0.0;
  }
  if(down_flag){
    if((height_point-3000) < wheel_1.getPulses())power[0] -= 0.6;
    else power[0] = 0.0;
    if((height_point-3000) < wheel_2.getPulses())power[1] -= 0.6;
    else power[1] = 0.0;
    if(power[0]+power[1] == 0.0){
        down_flag = false;
        end_flag = false;
    }
  }
}
void iti_checker()
{
  int X_;
  if(itidata[1]>=(1<<7)){
    itidata[1]-=(1<<7);
    X_=-1*(itidata[0]+(itidata[1]<<8));
  }
  else X_= itidata[0]+(itidata[1]<<8);
  
  if(X_ <= -1000)motor[2].setSpeed(-0.2);
  else motor[2].setSpeed(0.0);    
    
}
int main()
{
    sita_s.setHeaders('A','Z');
    sita_s.startReceive(1);
    ue_s.setHeaders('A','Z');
    ue_s.startReceive(5);
    /*
    IR0.startAveraging(5);
    IR1.startAveraging(5);
    IR2.startAveraging(5);*/

    pid.setInputLimits(-10000.0,10000.0);
    pid.setOutputLimits(-1.0,1.0);
    pid.setBias(0);
    pid.setMode(1);
    pid.setSetPoint(0.0);
    
    field = mode_r_b.read();
    debug_led_1 = field;
    debug_led_2 = !field;
    while(1)
    {
        for(int i = 0;i < 2;i++)power[i] = 0.0;
        debug_led_0 = !debug_led_0;
        //ir_checker();
        to_ue();
        to_sita();
        iti_checker();
        if(start.read() == 0)mode = 1;
        if(mode==1&&s_mode ==2)mode = 2;
        if(mode == 2&&(u_mode == 0xff || s_mode == 0xff))mode = 0xff - 1;
        if((mode == 0xff - 1)&&u_mode == 0xff && s_mode == 0xff)mode = 3;

        
        if(mode == 3&&u_mode == 3 &&s_mode == 3)syoukou();
        if(mode == 3&&u_mode == 3 &&s_mode == 4)syoukou();
        if(mode == 3&&u_mode == 4 &&s_mode == 3)syoukou();
        if(mode == 3&&u_mode == 4 &&s_mode == 4)syoukou();
        
        if(mode == 3&&u_mode == 4 &&s_mode == 4&&!end_flag)mode = 5;
        
        if(mode == 1){
                if(wheel_1.getPulses() <= 1000)power[0] += 0.5;
                else power[0] = 0;
                if(wheel_2.getPulses() <= 1000)power[1] += 0.5;
                else power[1] = 0;
        }
        /*
        if(u_mode == 6&&s_mode==6){
            if((height_point-4000) < wheel_1.getPulses())power[0] -= 0.6;
            else power[0] = 0.0;
            if((height_point-4000) < wheel_2.getPulses())power[1] -= 0.6;
            else power[1] = 0.0;
        }
        */
        
        motor[0].setSpeed(power[0]);
        motor[1].setSpeed(power[1]);

        
        pc.printf("%d,%d,%d,%d,%.2f,%.2f,%d\n\r",s_mode,u_mode,wheel_1.getPulses(),wheel_2.getPulses(),power[0],power[1],mode_r_b.read());
        //pc.printf("%d\n\r",data_a);



    }


    return 0;
}