Stap 30:
[mw_shl_code=applescript,true]#include "MeOrion.h" #include 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(); } [/mw_shl_code]
