爬楼梯小车程序代码

#include "MeOrion.h"
#include <Wire.h>
 
MeGyro gyro;
MeDCMotor lifer1(PORT_1);  //升降1
MeDCMotor lifer2(PORT_2);  //升降2
MeDCMotor rw(M2);  //1号驱动轮
MeDCMotor fw(M1);  //4号驱动轮
MeLineFollower lineFinder1(PORT_7);  // 巡线
MeLimitSwitch upWard1(PORT_3, SLOT2);  // 1号升降限位开关
MeLimitSwitch forWard2(PORT_4, SLOT1);   // 4号轮子限位开关
MeLimitSwitch upWard2(PORT_4, SLOT2);  // 2号升降限位开关
MeUltrasonicSensor ultraSensor(PORT_8);  // 1号轮子超声波
 
void setup()  // 初始化,前后升降装置回收
{
  Serial.begin(9600); 
  gyro.begin();
  lifer1.run(-250);
  while (!upWard1.touched());
  lifer1.stop();
  lifer2.run(-250);
  while (!upWard2.touched());
  lifer2.stop();
}
 
 
 
void Step2()
{
    float p = 15;
    float i = 0.01;
    float angle = 0;
    float targetAngle = 0;
    float integration = 0;
    float err = 0;
     
    while (NotStop())
    {
        gyro.update();
        float angle = gyro.getAngleX();
        err = angle - targetAngle;
        integration += err;
         
        int speed1 = 200;
        int speed2 = 200;
         
        speed2 = speed1 + err * p;// + integration * i;
         
        lifer1.run(speed1);
        lifer2.run(speed2);
        //delay(10);
    }
    lifer1.stop();
    lifer2.stop();
}
 
int startTiming = 0;
unsigned long startTime = 0;
 
int NotStop()
{
    int16_t distance;
    distance = ultraSensor.distanceCm();
    delay(10);
    if (0 == startTiming)
    {
        if ((distance < 10) && (distance > 1))
        {
            return 1;
        }
        else
        {
            startTiming = 1;
            startTime = millis();
            return 1;
        }
    }
    else
    {
        unsigned long currentTime = millis();
        if (currentTime - startTime < 2500)  //超声波检测不到楼梯后延时2.5秒
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }
}
 
 
void loop()
{
  startTiming = 0;
  startTime = 0;
//阶段1 小车前进
  fw.run(60); 
  rw.run(60);  //1号 4号轮子推动小车前进
 
  int16_t distance;
  do
  {
      distance = ultraSensor.distanceCm();
      delay(10);
  }
//阶段2 碰到楼梯 主动轮停止 整体车架上升
  while (!((distance < 4) && (distance > 1)));  //如果碰到楼梯
   
  fw.stop();
  rw.stop();
  Step2();
//阶段3 1号 4号轮子推动小车前进
  fw.run(60);
  rw.run(60);
//阶段4 2号轮子巡线传感器检测到楼梯 主动轮停止 升降装置1把2号轮子收回去
  while (!(lineFinder1.readSensors() != S1_IN_S2_IN));
  lifer1.run(-250);
  fw.stop();
  rw.stop();
   
  while (!upWard1.touched());  //1号升降限位开关检测升降结束
  lifer1.stop();
 
  Adjust();
//阶段5 1号 4号轮子推动小车前进 
  fw.run(60);
  rw.run(60);
//阶段6 4号轮子限位开关检测到4号轮子碰到楼梯 主动轮停止 2号升降装置回收4号轮子
  while (!forWard2.touched());
  lifer2.run(-250);
  fw.stop();
  rw.stop();
    
  while (!upWard2.touched()); //1号升降限位开关检测升降结束
  lifer2.stop();
//阶段7 循环阶段1 
//阶段8 循环阶段2 
}
 
void Adjust() //陀螺仪控制1、2号升降电机控制平衡
{
    int p = 40;
    //float i = 0.05;
    float angle = 0;
    float targetAngle = 4;
    //float integration = 0;
    //float err = 0;
     
    do
    {
        gyro.update();
        angle = gyro.getAngleX();
        //err = (angle - targetAngle);
        //integration += err;
        lifer2.run((angle - targetAngle) * p);// + i * integration);
        //delay(10);
    }
    while(fabs(angle - targetAngle) > 1);
 
    lifer2.stop();
}

 


代码来源:
http://bbs.makeblock.com/forum.php?mod=viewthread&fid=41&tid=852&highlight=%E7%88%AC%E6%A5%BC%E6%A2%AF
  



最后修改: 2018年06月29日 Friday 14:22