Kod: Zaznacz cały
#include <mbed.h>
#include "ArduinoMotorShield.hpp"
#include "ultrasonic.h"
#include "PinDetect.h"
/*
DigitalOut F_Left(D12);
DigitalOut F_Right(D13);
DigitalOut B_Left(D8);
DigitalOut B_Right(D9);
DigitalOut start_Left(D11);
DigitalOut start_Right(D4);
*/
Timer count , count2, coun3 ;
//ArduinoMotorShield engine;
AnalogIn sensor_light(PC_3);
Serial Bluetooth(PC_4, PC_5);
PinDetect left_sensor(PC_6);
PinDetect right_sensor(PC_8);
static int left_sensor_state = 0;
static int right_sensor_state = 0;
#define on 1
#define off 0
bool lock = true;
class Gear
{
public:
void manoeuvre(float step_left, float step_right)
{
_driver.SetMotorPower(ArduinoMotorShield::MOTOR_A, step_left);
_driver.SetMotorPower(ArduinoMotorShield::MOTOR_B, step_right);
}
void cramp(int brake_left, int brake_right)
{
_driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
_driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
}
private:
ArduinoMotorShield _driver;
};
class Miernik
{
public:
Miernik(PinName pin)
: _analog(pin)
{
}
float xx()
{
return _analog.read();
}
private:
AnalogIn _analog;
};
class connector
{
public:
connector(PinName pin)
: _pin(pin)
{
_pin = 0;
}
void state(int s)
{
_pin = s;
}
void toggle()
{
_pin = !_pin;
}
private:
DigitalOut _pin;
};
class bumper
{
public:
bumper(PinName pin)
: _clicker(pin)
{
}
void intro(jakis_argument)
{
_clicker.attach_deasserted(jakis_argument);
}
private:
PinDetect _clicker;
};
connector Led_front_right(PB_15);
connector Led_front_left(PB_1);
connector Led_back_right(PB_2);
connector Led_back_left(PB_11);
connector Buzzer(PB_12);
Miernik pomiar(PC_3);
Gear engine;
bumper zderzak(PC_5)
void(*bips)();
/*
void collision_sensor()
{
if (right_sensor_state == 1)
{
lock = false;
coun3.start();
engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.3);
if (coun3.read() >= 5)
{
right_sensor_state = 0;
lock = true;
coun3.stop();
coun3.reset();
}
}
if (left_sensor_state == 1)
{
lock = false;
coun3.start();
engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.3);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
if (coun3.read() >= 5)
{
left_sensor_state = 0;
lock = true;
coun3.stop();
coun3.reset();
}
}
}
*/
void dist(int distance)
{
/*
if (lock == true)
{
if ((distance <= 30) && (distance >= 0))
{
engine.SetBrake(ArduinoMotorShield::MOTOR_A, 1);
engine.SetBrake(ArduinoMotorShield::MOTOR_B, 1);
bips();
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.0);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.0);
count2.start();
if (count2.read_ms() >= 3000)
{
engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
if (count2.read_ms() >= 5000)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
}
if ((count2.read_ms() >= 7000) && (distance <= 50))
{
count2.stop();
count2.reset();
}
}
}
else if (distance <= 50)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.2);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.2);
}
else if (distance <= 60)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.3);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.3);
}
else if (distance <= 70)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.4);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.4);
}
else if (distance <= 80)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.5);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.5);
}
else if (distance <= 90)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.6);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
}
else if (distance <= 100)
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.7);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.7);
}
else
{
engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.8);
engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.8);
engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
}
*/
Bluetooth.printf("Distance %d mm\r\n", distance);
}
void right_sensor_push()
{
right_sensor_state = 1;
}
void left_sensor_push()
{
left_sensor_state = 1;
}
ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
void bip()
{
count.start();
if (count.read_ms() >= 500)
{
Led_back_left.toggle();
Buzzer.toggle();
count.stop();
count.reset();
}
}
int main()
{
zderzak.intro(&left_sensor_push);
/*
//engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
//engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
bips = &bip;
left_sensor.mode(PullUp);
left_sensor.attach_deasserted( &left_sensor_push);
left_sensor.setSampleFrequency();
right_sensor.mode(PullUp);
right_sensor.attach_deasserted(&right_sensor_push);
right_sensor.setSampleFrequency();
sensor.startUpdates();
*/
for (;;)
{
sensor.checkDistance();
engine.cramp(off, off);
//bip();
//collision_sensor();
Led_front_right.state(0);
Led_front_left.state(0);
Led_back_right.state(0);
//Led_back_left.state(1);
engine.manoeuvre(-0.3, 0.3);
wait(1);
engine.cramp(on,on);
Bluetooth.printf("This program runs since %1.2fv seconds.\n", pomiar.xx());
}
}