Hur man bygger en Stairbot med Makeblock (30 / 32 steg)
Steg 30:
[mw_shl_code = applescript, sanna] #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);
medan (! upWard1.touched());
lifer1.stop();
lifer2.Run(-250);
medan (! upWard2.touched());
lifer2.stop();
}
void Step2()
{
flyta p = 15.
flyta jag = 0,01;
float vinkel = 0;
flyta targetAngle = 0;
flyta integration = 0;
float err = 0;
samtidigt (NotStop())
{
gyro.Update();
float vinkel = gyro.getAngleX();
ERR = vinkel - targetAngle;
integration += fela;
int ET1 = 200.
int speed2 = 200.
SPEED2 = ET1 + fela * p; / / + integration * i.
lifer1.Run(Speed1);
lifer2.Run(SPEED2);
Delay(10);
}
lifer1.stop();
lifer2.stop();
}
int startTiming = 0;
osignerade långa startTime = 0;
int NotStop()
{
int16_t avstånd;
avståndet = ultraSensor.distanceCm();
Delay(10);
om (0 == startTiming)
{
om ((distance < 10) & & (avstånd > 1))
{
tillbaka 1.
}
annat
{
startTiming = 1;
startTime = millis();
tillbaka 1.
}
}
annat
{
osignerade långa currentTime = millis();
om (currentTime - startTime < 2500) //超声波检测不到楼梯后延时2.5秒
{
tillbaka 1.
}
annat
{
Return 0;
}
}
}
void loop()
{
startTiming = 0;
startTime = 0;
阶段1 小车前进
FW.Run(60);
RW.Run(60); 1号 4号轮子推动小车前进
int16_t avstånd;
göra
{
avståndet = ultraSensor.distanceCm();
Delay(10);
}
阶段2 碰到楼梯 主动轮停止 整体车架上升
medan (! () (Distance < 4) & & (avstånd > 1))); 如果碰到楼梯
FW.stop();
RW.stop();
Step2();
阶段3 1号 4号轮子推动小车前进
FW.Run(60);
RW.Run(60);
阶段4 2号轮子巡线传感器检测到楼梯 主动轮停止 升降装置1把2号轮子收回去
medan (! () lineFinder1.readSensors()! = S1_IN_S2_IN));
lifer1.Run(-250);
FW.stop();
RW.stop();
medan (! upWard1.touched()); 1号升降限位开关检测升降结束
lifer1.stop();
ADJUST();
阶段5 1号 4号轮子推动小车前进
FW.Run(60);
RW.Run(60);
阶段6 4号轮子限位开关检测到4号轮子碰到楼梯 主动轮停止 2号升降装置回收4号轮子
medan (! forWard2.touched());
lifer2.Run(-250);
FW.stop();
RW.stop();
medan (! upWard2.touched()); 1号升降限位开关检测升降结束
lifer2.stop();
阶段7 循环阶段1
阶段8 循环阶段2
}
void Adjust() //陀螺仪控制1、2号升降电机控制平衡
{
int p = 40;
flyta jag = 0,05;
float vinkel = 0;
flyta targetAngle = 4;
flyta integration = 0;
float err = 0;
göra
{
gyro.Update();
vinkel = gyro.getAngleX();
ERR = (vinkel - targetAngle);
integration += fela;
lifer2.Run ((angle-targetAngle) * p); / / + i * integration);
Delay(10);
}
medan (Folkesson (vinkel - targetAngle) > 1);
lifer2.stop();
}
[/ mw_shl_code]