void AP_EPM::init()
{
if (EPM_PIN_1 != -1 && EPM_PIN_2 != -1) {
hal.gpio->pinMode(EPM_PIN_1, GPIO_OUTPUT);
hal.gpio->pinMode(EPM_PIN_2, GPIO_OUTPUT);
neutral();
}
}
void AP_EPM::init()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
// initialise the EPM to the neutral position
neutral();
}
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void AP_Gripper_EPM::update_gripper()
{
// move EPM PWM output back to neutral after the last grab or release
if (AP_HAL::millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS) {
if (config.state == AP_Gripper::STATE_GRABBING) {
neutral();
config.state = AP_Gripper::STATE_GRABBED;
} else if (config.state == AP_Gripper::STATE_RELEASING) {
neutral();
config.state = AP_Gripper::STATE_RELEASED;
}
}
// re-grab the cargo intermittently
if (config.state == AP_Gripper::STATE_GRABBED &&
(config.regrab_interval > 0) &&
(AP_HAL::millis() - _last_grab_or_release > ((uint32_t)config.regrab_interval * 1000))) {
grab();
}
}
void AP_Gripper_EPM::init_gripper()
{
#ifdef UAVCAN_NODE_FILE
_uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC);
// http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
#endif
// initialise the EPM to the neutral position
neutral();
}
// update - moves the pwm back to neutral after the timeout has passed
// should be called at at least 10hz
void AP_EPM::update()
{
// move EPM PWM output back to neutral 2 seconds after the last grab or release
if (_flags.active && (hal.scheduler->millis() - _last_grab_or_release > EPM_RETURN_TO_NEUTRAL_MS)) {
neutral();
}
// re-grab the cargo intermittently
if (_flags.grab && (_regrab_interval > 0) && (hal.scheduler->millis() - _last_grab_or_release > ((uint32_t)_regrab_interval * 1000))) {
grab();
}
}
void paint(QPainter &painter){
// const T w=painter.device()->width();
// const T h=painter.device()->height();
// const T s=0.5;
//QPointF origo(w2,h2);
QPointF origin(bx,by);
QPointF neutral(nx,ny);
QPointF foot(cx-body.cx,cy-body.cy);
QPointF target(tx-body.cx,ty-body.cy);
QPointF old(sx-body.cx,sy-body.cy);
QPen base(QBrush(Qt::NoBrush),0.02);
QPen white(base);
white.setWidthF(0.04);
white.setColor(QColor(enabled?(lift?"white":"gray"):"darkred"));
QPen green(base);
green.setWidthF(0.04);
green.setColor(QColor("green"));
QPen purple(base);
purple.setWidthF(0.01);
purple.setColor(QColor("purple").darker());
QPen red(base);
red.setWidthF(0.02);
red.setColor(QColor(balance?"yellow":"red"));
QPen teal(base);
teal.setWidthF(0.01);
teal.setColor(QColor("teal"));
QPen blue(base);
blue.setWidthF(0.01);
blue.setColor(QColor("blue"));
const T r=0.02;
painter.setPen(teal);
painter.drawEllipse(old,r,r);
painter.setPen(blue);
painter.drawEllipse(target,r,r);
painter.setPen(purple);
painter.drawEllipse(neutral,r*0.3,r*0.3);
painter.setPen(red);
painter.drawEllipse(foot,r,r);
painter.setPen(white);
painter.drawLine(origin,foot);
painter.setPen(enabled?green:red);
painter.drawEllipse(origin,r,r);
}
int main(void)
{
neutral();
init();
lcdWriteStr(" ", 0, 0);
lcdWriteStr(" ", 1, 0);
trackLineInit();
while(1)
{
trackLine();
}
/*
// Bonus ball pickup sequence:
setPose(DRIVE_UP);
myDelay(30);
grabBonusBall();
*/
/*
// Grounb ball pickup sequence:
setPose(DRIVE_OPEN);
// move, pickup, and brake
forward();
myDelay(60);
setServo(LIFT, MAX_LIFT_UP);
//disableServo(LIFT);
myDelay(60);
brake();
myDelay(10);
// drop balls and return to initial position
setServo(DOOR, DOOR_OPEN);
myDelay(60);
setServo(DOOR, DOOR_CLOSED);
myDelay(30);
setServo(LIFT, MAX_LIFT_OPEN);
//myDelay(5);
//disableServo(DOOR);
//disableServo(LIFT);
*/
//lcdWriteStr("End", 1, 0);
brake();
return 0;
}
void AP_EPM::init()
{
// return immediately if not enabled
if (!_enabled) {
return;
}
#ifdef UAVCAN_NODE_FILE
_uavcan_fd = open(UAVCAN_NODE_FILE, 0);
// http://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html
::printf("EPM: UAVCAN fd %d\n", _uavcan_fd);
#endif
// initialise the EPM to the neutral position
neutral();
}
int main(void) {
configure_ports();
a2dInit();
a2dSetPrescaler(ADC_PRESCALE_DIV32);
a2dSetReference(ADC_REFERENCE_AVCC);
init_servos();
LED_on();
neutral();
hold_pos();
while (1) {
move_forward();
// sustain_pos();
}
return 0;
}
int main(int argc, char** argv) {
comPort=openPort(portS);//***PUT YOUR COM PORT NUMBER HERE!***
// location of the machine running the 3D motion cube
std::string receiverHost = "localhost";
if (argc > 2) {
std::cout << "Usage: " << argv[0] << " <hostname>" << std::endl;
std::cout << "The arguments specify the host of the motion cube (Default: localhost)" << std::endl;
return 1;
}
if (argc > 1) {
receiverHost = std::string(argv[1]);
}
EmoEngineEventHandle eEvent = EE_EmoEngineEventCreate();
EmoStateHandle eState = EE_EmoStateCreate();
unsigned int userID = 0;
try {
// if (EE_EngineConnect() != EDK_OK) {
if (EE_EngineRemoteConnect("127.0.0.1", 3008) != EDK_OK) {
throw std::exception("Emotiv Engine start up failed.");
}
else {
std::cout << "Emotiv Engine started!" << std::endl;
neutral();//send all servos to their neutral positions
}
int startSendPort = 6868;
std::map<unsigned int, SocketClient> socketMap;
promptUser();
while (true) {
// Handle the user input
if (_kbhit()) {
if (!handleUserInput()) {
break;
}
}
if(paused == true){
int state = EE_EngineGetNextEvent(eEvent);
// New event needs to be handled
if (state == EDK_OK) {
EE_Event_t eventType = EE_EmoEngineEventGetType(eEvent);
EE_EmoEngineEventGetUserId(eEvent, &userID);
switch (eventType) {
// New headset connected, create a new socket to send the animation
case EE_UserAdded:
{
std::cout << std::endl << "New user " << userID << " added, sending Cognitiv animation to ";
std::cout << receiverHost << ":" << startSendPort << "..." << std::endl;
promptUser();
socketMap.insert(std::pair<unsigned int, SocketClient>(
userID, SocketClient(receiverHost, startSendPort, UDP)));
startSendPort++;
break;
}
// Headset disconnected, remove the existing socket
case EE_UserRemoved:
{
std::cout << std::endl << "User " << userID << " has been removed." << std::endl;
promptUser();
std::map<unsigned int, SocketClient>::iterator iter;
iter = socketMap.find(userID);
if (iter != socketMap.end()) {
socketMap.erase(iter);
}
break;
}
// Send the Cognitiv animation if EmoState has been updated
case EE_EmoStateUpdated:
{
//std::cout << "New EmoState from user " << userID << "..." << std::endl;
EE_EmoEngineEventGetEmoState(eEvent, eState);
std::map<unsigned int, SocketClient>::iterator iter;
iter = socketMap.find(userID);
if (iter != socketMap.end()) {
sendCognitiv(eState);
}
break;
}
// Handle Cognitiv training related event
case EE_CognitivEvent:
{
handleCognitivEvent(std::cout, eEvent);
break;
}
default:
break;
}
}
else if (state != EDK_NO_EVENT) {
std::cout << "Internal error in Emotiv Engine!" << std::endl;
break;
}
}
Sleep(1);
}
}
catch (const std::exception& e) {
std::cerr << e.what() << std::endl;
std::cout << "Press any keys to exit..." << std::endl;
getchar();
}
EE_EngineDisconnect();
EE_EmoStateFree(eState);
EE_EmoEngineEventFree(eEvent);
return 0;
}
void init() final override { _result = neutral(); }
