void fireGolfBall(void){
setRun(ROLLER_MOTOR_INDEX, 1);
usleep(microsecondsToMilliseconds(500));
dropGolfBall();
usleep(microsecondsToMilliseconds(500));
setRun(ROLLER_MOTOR_INDEX, 0);
}
int main(int argc, char **argv)
{
char buf[10];
setSignalAction();
startThread();
while (!getExitApp()) {
printf("Press return to start recording.\n");
getchar();
setRun(1);
pthread_barrier_wait(&startBarrier);
printf("Press return to end recording.\n");
getchar();
setRun(0);
pthread_barrier_wait(&endBarrier);
printf("Recording stopped.\n");
}
joinThread();
return 0;
}
void aimAtTargetFORWARD(int error)
{
printf("Attempting to aim at target! Error: %d\n", error);
setRun(RIGHT_WHEEL_INDEX, 1);
setRun(LEFT_WHEEL_INDEX, 1);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY - (error * 50) + (ADJUST_RATE / 2));
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY + (error * 50) - (ADJUST_RATE / 2));
}
void fireGolfBalls (int num){
int i;
setRun(ROLLER_MOTOR_INDEX, 1);
usleep(microsecondsToMilliseconds(1000));
for(i = 0; i < num; i++){
dropGolfBall();
}
usleep(microsecondsToMilliseconds(250));
setRun(ROLLER_MOTOR_INDEX, 0);
}
void AniTest::runThisTest()
{
Director::getInstance()->replaceScene(this);
initAni();
// Add Touch Listener
auto dispatcher = Director::getInstance()->getEventDispatcher();
auto touchListener = EventListenerTouchOneByOne::create();
touchListener->onTouchBegan = [=](Touch* touch, Event* event){
m_armature->stopAllActions();
Point touchPoint = touch->getLocation();
Point heroPoint = m_armature->getPosition();
Point deltaPoint = touchPoint - heroPoint;
float time = sqrt(deltaPoint.x * deltaPoint.x + deltaPoint.y * deltaPoint.y) * 0.002;
ActionInterval *action = cocos2d::MoveBy::create(time, deltaPoint);
FiniteTimeAction *actionCallBack = CallFuncN::create(CC_CALLBACK_1(AniTest::setNormal, this));
m_armature->runAction(Sequence::create(action, actionCallBack, NULL));
setRun();
if(deltaPoint.x < 0){
m_armature->setRotation3D(Vertex3F(0, 180, 0));
}else
{
m_armature->setRotation3D(Vertex3F(0, 0, 0));
}
return true;
};
dispatcher->addEventListenerWithSceneGraphPriority(touchListener, this);
}
bool CEnemySprite::init(int iType, int startType, int moveType)
{
bool bRet = false;
do{
CC_BREAK_IF(! CCLayer::init());
m_iType = iType;
mStartType = startType;
m_iMoveType = moveType;
m_bDestroy = false;
m_iDirection = STRAIGHT;
memset(&m_szName, 0, sizeof(m_szName));
// m_pSprite = CCSprite::createWithSpriteFrameName("Plane1_0.png");
// m_pSprite = CCSprite::createWithSpriteFrameName("plane_small_01_a.png");
// m_pSprite = CCSprite::create("new/plane_02.png");
// m_pSprite->setFlipY(true);
setAttr();
// setFlyAnimation(m_iDirection);
/** ÉèÖõÐÈËÒƶ¯¹ì¼£ */
setRun(0);
this->scheduleUpdate();
this->schedule(schedule_selector(CEnemySprite::shoot), m_fShootSpace);
this->schedule(schedule_selector(CEnemySprite::changeFlyAnimation), 0.1f);
bRet = true;
}
while(0);
return bRet;
}
void trigger()
{
setRun(true);
if(IsRunning() == false)
{
Start();
}
}
void startThread()
{
printf("Starting listen thread...\n");
setExitApp(0);
setRun(0);
pthread_barrier_init(&startBarrier, NULL, 2);
pthread_barrier_init(&endBarrier, NULL, 2);
pthread_mutex_init(&runMutex, NULL);
pthread_create(&thread, NULL, listenThread, NULL);
}
faceThread::faceThread(QObject *parent) :
QThread(parent)
{
facesketch = new faceSketch();
runFlag = 1;
CHANGE_SPEED = 0.1;
UPBOUND = 1;
LOWBOUND = -1;
timer = new QTimer();
connect(timer,SIGNAL(timeout()),this,SLOT(setRun()));
timer->start(100);
setSleep();
}
void aimAtTarget(double error){
if (error == 0){
return;
}
if (error < 0){
//Turn left
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE);
}
else if (error > 0){
//Turn right
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE);
}
setRun(RIGHT_WHEEL_INDEX, 1);
setRun(LEFT_WHEEL_INDEX, 1);
usleep(1250 * abs(error));
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
}
void VideoItem::keyEvent(QKeyEvent *event)
{
bool is_pressed = (event->type() == QEvent::KeyPress) ? true : false;
switch(event->key()) {
case Qt::Key_Escape:
if(is_pressed) {
emit setWindowedChanged(true);
event->accept();
}
break;
case Qt::Key_Space:
if(is_pressed) {
setRun(m_run ? false : true);
event->accept();
}
break;
}
}
/* 初期化: プログラム起動時に一度だけ実行 */
void initialize()
{
void setRun();
/*******↓for server mode *******/
if (s_mode) {
server_init( nPort );
}
/*******↑***********************/
#ifndef WIN32
setpgrp();
#endif
set_da_signal();
init_slot_prop();
init_text_analysis();
init_hmmsynth();
read_phonemes( phlist_file );
read_dic( dic_file );
init_tag();
init_mora();
init_morph();
init_aphrase();
init_breath();
init_phoneme();
init_sentence();
strcpy( slot_Speak_stat, "IDLE" );
setRun( "=", "LIVE" );
strcpy( slot_Log_file, "NO" ); logfp = NULL;
slot_Log_chasen= slot_Log_tag = slot_Log_phoneme = 0;
slot_Log_mora = slot_Log_morph = slot_Log_aphrase = 0;
slot_Log_breath = slot_Log_sentence = 0;
strcpy( slot_Err_file, "CONSOLE" );
slot_Speech_file[0] = '\0';
slot_Pros_file[0] = '\0';
prosBuf.nPhoneme = 0;
slot_Speak_syncinterval = 1000;
}
KonqView::~KonqView()
{
//kDebug() << "part=" << m_pPart;
// We did so ourselves for passive views
if (m_pPart != 0L)
{
finishedWithCurrentURL();
if ( isPassiveMode() )
disconnect( m_pPart, SIGNAL( destroyed() ), m_pMainWindow->viewManager(), SLOT( slotObjectDestroyed() ) );
if (m_pPart->manager())
m_pPart->manager()->removePart(m_pPart); // ~Part does this, but we have to do it before (#213876, #207173)
delete m_pPart;
}
qDeleteAll( m_lstHistory );
m_lstHistory.clear();
setRun( 0L );
//kDebug() << this << "done";
}
int main( int argc, char **argv )
{
int n, i;
char *com;
fp_err = stderr;
init_conf();
com = argv[0];
--argc; ++argv;
while( argc > 0 && argv[0][0] == '-' ) {
switch( argv[0][1] ) {
case 'C':
if( argc < 2 ) usage( com );
read_conf( argv[1] );
--argc; ++argv;
break;
/*******↓for server mode *******/
case 'p':
/* 引数が不正な場合はエラー出力 */
if( argc < 2 ) usage( com );
/* ポート番号の読み込み */
i = atoi( argv[1] );
if (i > 1024) {
nPort = i;
}
s_mode = 1;
--argc; ++argv;
break;
/*******↑***********************/
case 'v':
printf( "%s\n", moduleVersion );
printf( "%s\n", protocolVersion );
exit(0);
default:
usage( com );
}
--argc; ++argv;
}
set_default_conf();
initialize();
n = setjmp( ebuf );
if( n > 0 ) chasen_process = 0; /* to restart 'chasen' process */
for( ;; ) {
#ifdef PRINTDATA
TmpMsg( "> " );
#endif
n_arg = read_command( v_arg );
#ifdef PRINTDATA
{
int i;
TmpMsg( "command is \n" );
for( i=0; i<n_arg; ++i ) {
TmpMsg( " %d: %s\n", i+1, v_arg[i] );
}
}
#endif
/* 「o」 で set Speak = NOW のショートカット */
if( strcmp(v_arg[0],"o")==0 ) {
setSpeak( "=", "NOW" );
continue;
}
if( n_arg < 2 ) { unknown_com(); continue; }
switch( commandID( v_arg[0] ) ) {
case C_set:
if( n_arg < 4 ) { unknown_com(); break; }
switch( slotID( v_arg[1] ) ) {
case S_Run: setRun( v_arg[2], v_arg[3] ); break;
case S_Speaker: setSpeaker( v_arg[2], v_arg[3] ); break;
case S_Alpha: setAlpha( v_arg[2], v_arg[3] ); break;
case S_Postfilter_coef: setPostfilter_coef( v_arg[2], v_arg[3] ); break;
case S_Text: setText( v_arg[2], v_arg[3] ); break;
case S_Speak: setSpeak( v_arg[2], v_arg[3] ); break;
case S_SaveRAW: setSave( v_arg[2], v_arg[3] ); break;
case S_Save: setSave( v_arg[2], v_arg[3] ); break;
case S_LoadRAW: setSpeechFile( v_arg[2], v_arg[3], RAW ); break;
case S_SpeechFile: setSpeechFile( v_arg[2], v_arg[3], RAW ); break;
case S_SaveWAV: setSaveWAV( v_arg[2], v_arg[3] ); break;
case S_LoadWAV: setSpeechFile( v_arg[2], v_arg[3], WAV ); break;
case S_SavePros: setSavePros( v_arg[2], v_arg[3] ); break;
case S_LoadPros: setProsFile( v_arg[2], v_arg[3] ); break;
case S_ProsFile: setProsFile( v_arg[2], v_arg[3] ); break;
case S_ParsedText: setParsedText( v_arg[2], v_arg[3] ); break;
case S_Speak_syncinterval: setSpeakSyncinterval( v_arg[2], v_arg[3] ); break;
case S_AutoPlay:
slot_Auto_play = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_AutoPlayDelay:
slot_Auto_play_delay = atoi( v_arg[3] ); break;
case S_Log: setLog( v_arg[2], v_arg[3] ); break;
case S_Log_conf:
slot_Log_conf = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_text:
slot_Log_text = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_arranged_text:
slot_Log_arranged_text = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_chasen:
slot_Log_chasen = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_tag:
slot_Log_tag = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_phoneme:
slot_Log_phoneme = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_mora:
slot_Log_mora = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_morph:
slot_Log_morph = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_aphrase:
slot_Log_aphrase = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_breath:
slot_Log_breath = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Log_sentence:
slot_Log_sentence = setLogYesNo( v_arg[2], v_arg[3] ); break;
case S_Err: setErr( v_arg[2], v_arg[3] ); break;
default:
unknown_com();
}
break;
case C_inq:
switch( slotID( v_arg[1] ) ) {
case S_Run: inqRun(); break;
case S_ModuleVersion: inqModuleVersion(); break;
case S_ProtocolVersion: inqProtocolVersion(); break;
case S_SpeakerSet: inqSpeakerSet(); break;
case S_Speaker: inqSpeaker(); break;
case S_SpeechFile: inqSpeechFile(); break;
case S_ProsFile: inqProsFile(); break;
case S_AutoPlay: inqAutoPlay(); break;
case S_AutoPlayDelay: inqAutoPlayDelay(); break;
case S_Text_text: inqTextText(); break;
case S_Text_pho: inqTextPho(); break;
case S_Text_dur: inqTextDur(); break;
case S_Speak_text: inqSpeakText(); break;
case S_Speak_pho: inqSpeakPho(); break;
case S_Speak_dur: inqSpeakDur(); break;
case S_Speak_utt: inqSpeakUtt(); break;
case S_Speak_len: inqSpeakLen(); break;
case S_Speak_stat: inqSpeakStat(); break;
case S_Speak_syncinterval: inqSpeakSyncinterval(); break;
case S_Log:
RepMsg( "rep Log = %s\n", slot_Log_file ); break;
case S_Log_conf:
RepMsg( "rep Log.conf = %s\n", YesNoSlot(S_Log_conf) ); break;
case S_Log_text:
RepMsg( "rep Log.text = %s\n", YesNoSlot(S_Log_text) ); break;
case S_Log_arranged_text:
RepMsg( "rep Log.arranged_text = %s\n", YesNoSlot(S_Log_arranged_text) ); break;
case S_Log_chasen:
RepMsg( "rep Log.chasen = %s\n", YesNoSlot(S_Log_chasen) ); break;
case S_Log_tag:
RepMsg( "rep Log.tag = %s\n", YesNoSlot(S_Log_tag) ); break;
case S_Log_phoneme:
RepMsg( "rep Log.phoneme = %s\n", YesNoSlot(S_Log_phoneme) ); break;
case S_Log_mora:
RepMsg( "rep Log.mora = %s\n", YesNoSlot(S_Log_mora) ); break;
case S_Log_morph:
RepMsg( "rep Log.morph = %s\n", YesNoSlot(S_Log_morph) ); break;
case S_Log_aphrase:
RepMsg( "rep Log.aphrase = %s\n", YesNoSlot(S_Log_aphrase) ); break;
case S_Log_breath:
RepMsg( "rep Log.breath = %s\n", YesNoSlot(S_Log_breath) ); break;
case S_Log_sentence:
RepMsg( "rep Log.sentence = %s\n", YesNoSlot(S_Log_sentence) ); break;
case S_Err:
RepMsg( "rep Err = %s\n", slot_Err_file ); break;
default:
unknown_com();
}
break;
case C_prop:
{ SlotProp prop;
if( strcmp(v_arg[2],"=")!=0 ) { unknown_com(); break; }
if( strcmp(v_arg[3],"AutoOutput")==0 ) {
prop = AutoOutput;
} else if(strcmp(v_arg[3],"NoAutoOutput")==0 ) {
prop = NoAutoOutput;
} else {
unknown_com(); break;
}
switch( slotID( v_arg[1] ) ) {
case S_Run: prop_Run = prop; break;
case S_ModuleVersion: prop_ModuleVersion = prop; break;
case S_ProtocolVersion: prop_ProtocolVersion = prop; break;
case S_SpeakerSet: prop_SpeakerSet = prop; break;
case S_Speaker: prop_Speaker = prop; break;
case S_SpeechFile: prop_SpeechFile = prop; break;
case S_ProsFile: prop_ProsFile = prop; break;
case S_Text: prop_Text = prop; break;
case S_Text_text: prop_Text_text = prop; break;
case S_Text_pho: prop_Text_pho = prop; break;
case S_Text_dur: prop_Text_dur = prop; break;
case S_Speak: prop_Speak = prop; break;
case S_Speak_text: prop_Speak_text = prop; break;
case S_Speak_pho: prop_Speak_pho = prop; break;
case S_Speak_dur: prop_Speak_dur = prop; break;
case S_Speak_utt: prop_Speak_utt = prop; break;
case S_Speak_len: prop_Speak_len = prop; break;
case S_Speak_stat: prop_Speak_stat = prop; break;
case S_Speak_syncinterval: prop_Speak_syncinterval = prop; break;
default:
unknown_com();
}
}
break;
default:
unknown_com();
}
}
if( s_mode ) {
server_destroy ();
}
exit(0);
}
int read_command( char **args )
{
int n=0, p=0;
char *c;
if( s_mode ) {
if (server_getline( cline, MAX_COMMAND_LEN ) <= 0) {
args[0] = "";
return 0;
}
} else {
if( g_getline( cline, MAX_COMMAND_LEN ) < 0)
setRun( "=", "EXIT" );
if( ! strlen( cline) > 0) return 0;
}
c = cline;
/* to skip space */
while( *c==' ' || *c=='\t' ) { ++c; };
/* to get a command name */
*(args++) = c;
while( *c!=' ' && *c!='\t' && *c!='\n' && *c!= EOF ) {
c++;
}
*(c++) = '\0'; ++n;
/* to skip space */
while( *c==' ' || *c=='\t' ) { ++c; };
/* to get a slot name */
*(args++) = c;
while( *c!=' ' && *c!='\t' && *c!='=' && *c!='<' && *c!='\n' && *c!= EOF ) {
c++;
};
*(c++) = '\0'; ++n;
/* to skip space */
while( *c==' ' || *c=='\t' ) { ++c; };
/* to get relation */
*(args++) = c;
if( *c=='=' || *c=='<' ) {
c++;
if( *(c-1)=='<' && *c=='<' ) {
c++;
}
*(c++) = '\0'; ++n;
}
/* to skip space */
while( *c==' ' || *c=='\t' ) { ++c; };
/* to get a value */
*(args++) = c;
while( *c!='\n' && *c!='\0' && *c!= EOF ) {
c++;
};
*c = '\0'; ++n;
return n;
}
/*MAIN*/
int main()
{
//Declarations
int i, j;
int targets;
int center, error, most_blob;
int num_balls = 5;
int dist, prev;
int n = 0;
//Set up GPIO
gpio_export(POWER_LED_PIN);
gpio_set_dir(POWER_LED_PIN, OUTPUT_PIN);
gpio_export(CAMERA_LED_PIN);
gpio_set_dir(CAMERA_LED_PIN, OUTPUT_PIN);
gpio_export(BUTTON_PIN);
gpio_set_dir(BUTTON_PIN, INPUT_PIN);
gpio_export(RELOAD_LED_PIN);
gpio_set_dir(RELOAD_LED_PIN, OUTPUT_PIN);
gpio_export(ARDUINO_PIN);
gpio_set_dir(ARDUINO_PIN, OUTPUT_PIN);
gpio_set_value(ARDUINO_PIN, LOW);
//Set up PWM
initPWM();
usleep(microsecondsToMilliseconds(500));
//Dropper
setPeriod(DROPPER_MOTOR_INDEX, PWM_PERIOD);
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION));
setRun(DROPPER_MOTOR_INDEX, 1);
//Grabber
setPeriod(GRABBER_MOTOR_INDEX, PWM_PERIOD);
setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION);
setRun(GRABBER_MOTOR_INDEX, 1);
//Right wheel
setPeriod(RIGHT_WHEEL_INDEX, PWM_PERIOD);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(RIGHT_WHEEL_INDEX, 0);
//Left wheel
setPeriod(LEFT_WHEEL_INDEX, PWM_PERIOD);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(LEFT_WHEEL_INDEX, 0);
//Roller
setRun(ROLLER_MOTOR_INDEX, 0);
setPeriod(ROLLER_MOTOR_INDEX, PWM_PERIOD);
setDuty(ROLLER_MOTOR_INDEX, PWM_PERIOD*ROLLER_SPEED);
//Set up serial
serial* arduinoSerial;
if (serial_open(&arduinoSerial, ARDUINO_SERIAL_PORT, ARDUINO_BAUD_RATE) == 0){
printf("Opened Ardunio serial port sucessfully\n");
}
else{
printf("Failed to open Arduino serial port\n");
}
/*for(i = 0; i < 50; i++)
{
dist = getDistance(arduinoSerial);
printf("Instensity: %d\n", dist);
usleep(microsecondsToMilliseconds(100));
}
usleep(microsecondsToMilliseconds(5000));*/
//Turn on power LED
gpio_set_value(POWER_LED_PIN, HIGH);
printf("Systems are GO for launch!\n");
waitForButtonPress();
getDistance(arduinoSerial);
//Move to center of the board
goForward();
usleep(microsecondsToMilliseconds(1800));
stopWheels();
//Aim at right target
rotate(180);
if(aimAtSideMostTarget(1))
{
for (; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Move toward left target
rotate(-550);
goForward();
usleep(microsecondsToMilliseconds(2200));
stopWheels();
//Aim at left target
rotate(300);
if(aimAtSideMostTarget(0))
{
for (; num_balls > 1; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(260);
aimAtBiggestTarget();
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
//Move to sideline for reload
rotate(-600);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("%d, ", dist);
usleep(microsecondsToMilliseconds(9));
}
while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1100);
//Turn on reload LED
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
while (n < 3) //big while loop thing!
{
//start from left
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1200));
stopWheels();
rotate(-420);
goForward();
usleep(microsecondsToMilliseconds(590));
stopWheels();
//Aim at left target
if(aimAtSideMostTarget(0))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(420);
goForward();
usleep(microsecondsToMilliseconds(600));
stopWheels();
rotate(-150);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at right target
rotate(400);
goForward();
usleep(microsecondsToMilliseconds(2700));
stopWheels();
rotate(-370);
if(aimAtSideMostTarget(1))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(320);
//usleep(microsecondsToMilliseconds(50));
goForward();
buffer_clear(arduinoSerial);
dist = 0;
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("And another thing....\n");
//printf("%d!!! \n", dist);
usleep(microsecondsToMilliseconds(10));
}while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(-900);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
//start from right
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1350));
stopWheels();
rotate(570);
//Aim at right target
if(aimAtSideMostTarget(1))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(-500);
goForward();
usleep(microsecondsToMilliseconds(4500));
stopWheels();
rotate(650);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at left target
rotate(-200);
if(aimAtSideMostTarget(0))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(-400);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
usleep(microsecondsToMilliseconds(9));
}
while(dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1000);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
} //end of while loop
//Turn off gpio power
gpio_set_value(POWER_LED_PIN, LOW);
gpio_set_value(RELOAD_LED_PIN, LOW);
gpio_set_value(ARDUINO_PIN, LOW);
//Turn off motors
setRun(GRABBER_MOTOR_INDEX, 0);
setRun(DROPPER_MOTOR_INDEX, 0);
setRun(ROLLER_MOTOR_INDEX, 0);
setRun(RIGHT_WHEEL_INDEX, 0);
setRun(LEFT_WHEEL_INDEX, 0);
//Remove GPIO files
//gpio_unexport(PING_PIN);
return 0;
}
void turnOnRoller(void){
setRun(ROLLER_MOTOR_INDEX, 1);
}
void goForward(void){
setRun(LEFT_WHEEL_INDEX, 1);
setRun(RIGHT_WHEEL_INDEX, 1);
setDuty(LEFT_WHEEL_INDEX, MAXIMUM_DUTY);
setDuty(RIGHT_WHEEL_INDEX, MINIMUM_DUTY);
}
