void translateIR() { //Used when robot is switched to operate in remote control mode
switch(results.value)
{
case 0xFF629D: //Case 'FORWARD'
go();
break;
case 0xFF22DD: //Case 'LEFT'
turnleft(turntime);
stopmove();
break;
case 0xFF02FD: //Case 'OK'
stopmove();
break;
case 0xFFC23D: //Case 'RIGHT'
turnright(turntime);
stopmove();
break;
case 0xFFA857: //Case 'REVERSE'
backwards();
break;
case 0xFF42BD: //Case '*'
modecontrol=0; stopmove(); // If an '*' is received, switch to automatic robot operating mode
break;
default:
;
}// End Case
delay(500); // Do not get immediate repeat
}
int stFindIterImp::next()
{
if ( !state_.reverse() )
return forwards();
else
return backwards();
}
int main(void)
{
TRISA = 0x0060; // PORTA 6 & 7 are inputs and the rest are output
TRISB = 0x0000;
TRISD = 0x000;
PORTB = 0x0000; //
PORTD = 0x0000;
T2CON = 0x8040; //
PR2= 0xFFFF;
T1CON = 0x8030;
PR1 = 0xFFFF; // period to timer1. resets when it is full.
while(1)
{
if (PORTAbits.RA6 ==1)
{
forward(4, 9000);
stop(1);
backwards(3, 6000);
stop(2);
forward(3, 4000);
stop(1);
forward(5, 2000);
}
}
return 0;
}
// Returns a pointer to gcf(a,b) where
// a = x + [base computers work in] and
// b = x + [x written backwards] (for x
// written in base 10).
//
// where will the actual integer gcf(a,b) be stored?
// we can't have a pointer to a value on the stack, so
// it must be on the heap (via malloc)
int *f4(int x)
{
int a = x + 2;
int b = x + backwards(x);
int *gcf = (int *)malloc(sizeof(int));
memset(gcf, 0, sizeof(int));
// we can calculate gcf(a,b) using the Euclidian algorithm
*gcf = gcd(a, b);
return gcf;
}
void ExplicitConvolution::convolve(Complex *f, Complex *g)
{
pad(f);
backwards(f);
pad(g);
backwards(g);
double ninv=1.0/n;
#ifdef __SSE2__
Vec Ninv=LOAD(ninv);
#endif
PARALLEL(
#ifdef __SSE2__
for(unsigned int k=0; k < n; ++k)
STORE(f+k,Ninv*ZMULT(LOAD(f+k),LOAD(g+k)));
#else
for(unsigned int k=0; k < n; ++k)
f[k] *= g[k]*ninv;
#endif
)
void keypress(unsigned char key, int x, int y){
switch (key)
{
case 'q':
exit(0);
break;
case 'w':
forwards();
break;
case 's':
backwards();
break;
}
}
void main(void)
{
/*Variable Definitions*/
struct DC_motor motorL, motorR; //declare two DC_motor structures
motorL.power=0;
motorL.direction=1;
motorL.dutyLowByte=(unsigned char *)(&PDC1L);
motorL.dutyHighByte=(unsigned char *)(&PDC1H);
motorL.dir_pin=2;
motorL.PWMperiod=PWMcycle;
motorR.power=0;
motorR.direction=1;
motorR.dutyLowByte=(unsigned char *)(&PDC0L);
motorR.dutyHighByte=(unsigned char *)(&PDC0H);
motorR.dir_pin=0;
motorR.PWMperiod=PWMcycle;
struct DC_motor * m_L;
struct DC_motor * m_R;
m_L=&motorL; //setup pointer to left motor
m_R=&motorR; //setup pointer to right motor
/*Oscillator Setup*/
OSCCON = 0x72; //8MHz clock
while (!OSCCONbits.IOFS); //wait until stable
/*Setup Registers*/
TRISB = 0;
initPWM(); //setup PWM registers
while (1)
{
fullSpeedAhead(m_L,m_R);
delay_s(1);
stop(m_L,m_R);
turnLeft(m_L,m_R);
delay_s(1);
stop(m_L,m_R);
turnRight(m_L,m_R);
delay_s(1);
stop(m_L,m_R);
backwards(m_L,m_R);
delay_s(1);
stop(m_L,m_R);
}
}
xml_element* tchilds::last_element()const {
if( _childs.size() == 0 ) {
return nullptr;
}
for( xml_node * n : backwards( _childs ) ) {
xml_element* elem = dynamic_cast<xml_element*>( n );
if( elem != nullptr ) {
return elem;
}
}
return nullptr;
}
int main(int argc, char** argv){
int c = argc;
int j=0;
while(argv[1][j]!='\0'){
switch(argv[1][j]){
case 'b': backwards(argv,2,c-1);break;
case 'r': reverse(argv,2,c-1);break;
case 's': sort(argv,2,c-1);break;
default: break;
}
j++;
}
for(j = 2; j< c;j++){
printf("%s ",argv[j]);
}
printf("\n");
return 0;
}
//Start of Auto
task main()
{
waitForStart(); //waits for Starts
initializeRobot();
//Servos
armUp();
forward(6950, 50);
stopMotors();
forward(650, 20);
// drives down the ramp
armUp();
frontServoDown();
wait1Msec(500);
turnLeft(4700, 20);
wait1Msec(500);
//turns to face the other tube
armUp();
backServoUp();
wait1Msec(500);
backwards(2375, 15); //backwards for half the distance as we want
stopMotors();
backServoDown();
armDown();
PlaySound(soundBeepBeep);
wait1Msec(500);
armUp();
//scores balls
frontServoDown();
backServoDown();
wait1Msec(500);
turnRight(300, 30);
//backs into the second tube and grabs it
wait1Msec(500);
PlaySound(soundBeepBeep);
forward(9000, 50);
turnRight(300, 30);
forward(450, 50);
turnLeft(500, 30);
wait1Msec(500);
stopMotors();
PlaySound(soundBeepBeep);
forward(5000, 7.5); //forward(5000, 20); //makes sure we dont fall short.
}
/* Check a move for validity in the current state. If it is valid, it
* is applied via domove(), otherwise return FALSE. If the move is
* equivalent to an undo or a redo, then use that instead; otherwise,
* the redo list is reset.
*/
int newmove(int dir)
{
action move;
if (!state.currblock || !canmove(state.currblock, dir))
return FALSE;
if (state.undo.count) {
move = state.undo.list[state.undo.count - 1];
if (move.id == state.currblock && move.dir == backwards(dir)
&& !move.door)
return undomove();
}
if (state.redo.count) {
move = state.redo.list[state.redo.count - 1];
if (move.id == state.currblock && move.dir == dir)
return redomove();
}
domove(makeaction(state.currblock, dir));
state.redo.count = 0;
return TRUE;
}
/* Unapply the last move on the undo list, reversing what was done in
* domove() and adding the move to the redo list.
*/
int undomove(void)
{
action move;
if (!state.undo.count)
return FALSE;
move = state.undo.list[--state.undo.count];
addtomovelist(&state.redo, move);
moveblock(move.id, backwards(move.dir));
state.currblock = move.id;
state.ycurrpos = state.xcurrpos = 0;
--state.movecount;
if (!state.undo.count ||
move.id != state.undo.list[state.undo.count - 1].id)
--state.stepcount;
if (move.door)
resetdoors();
return TRUE;
}
bool Move::isPossible(const Board& b, Color player, int movesLeft) const
{
if(!isValid())
return false;
if(movesLeft < m_cost) //not enough moves left
return false;
if(b.isFrozen(m_position) || b.getPiece(m_position)->getColor() != player) //there is no piece to move (or it is frozen) or it is the wrong color
return false;
if(b.getPiece(m_position)->getType() == RABBIT) //if the piece is a rabbit, forbids it to go backwards
{
//figuring out where backwards is
Square backwards(0,0);
if(player == GOLD)
backwards.y -= 1;
else //SILVER
backwards.y += 1;
if(m_destination == m_position + backwards) //going backwards
return false;
}
return b.isFree(m_destination); //rturns true if the destination is free
}
xml_element* tchilds::last_element( std::string const& val )const {
if( _childs.size() == 0 ) {
return nullptr;
}
vector<xml_node*>::const_reverse_iterator rbegin = _childs.rbegin();
vector<xml_node*>::const_reverse_iterator rend = _childs.rend();
for( xml_node * n : backwards( _childs ) ) {
xml_element* elem = dynamic_cast<xml_element*>( n );
if( elem != nullptr ) {
if( elem->value() == val ) {
return elem;
}
}
}
return nullptr;
}
void controller()
{
if(x > 0)
{
right();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if (x < 0)
{
left();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(y > 0)
{
forwards();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(y < 0)
{
backwards();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(z > 0)
{
up();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(z < 0){
down();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
if(rotation > 0)
{
rotateLeft();
//digitalWrite(ledpin, HIGH); // turn ON the LED
}else if(rotation < 0)
{
rotateRight();
//digitalWrite(ledpin, LOW); // turn ON the LED
}
}
Func performBlur(Func f, Func coeff, Expr size, Expr sigma) {
Func blurred;
blurred(x, y) = undef<float>();
// Warm up
blurred(x, 0) = coeff(0) * f(x, 0);
blurred(x, 1) = (coeff(0) * f(x, 1) +
coeff(1) * blurred(x, 0));
blurred(x, 2) = (coeff(0) * f(x, 2) +
coeff(1) * blurred(x, 1) +
coeff(2) * blurred(x, 0));
// Top to bottom
RDom fwd(3, size - 3);
blurred(x, fwd) = (coeff(0) * f(x, fwd) +
coeff(1) * blurred(x, fwd - 1) +
coeff(2) * blurred(x, fwd - 2) +
coeff(3) * blurred(x, fwd - 3));
// Tail end
Expr padding = cast<int>(ceil(4*sigma) + 3);
RDom tail(size, padding);
blurred(x, tail) = (coeff(1) * blurred(x, tail - 1) +
coeff(2) * blurred(x, tail - 2) +
coeff(3) * blurred(x, tail - 3));
// Bottom to top
Expr last = size + padding - 1;
RDom backwards(0, last - 2);
Expr b = last - 3 - backwards; // runs from last - 3 down to zero
blurred(x, b) = (coeff(0) * blurred(x, b) +
coeff(1) * blurred(x, b + 1) +
coeff(2) * blurred(x, b + 2) +
coeff(3) * blurred(x, b + 3));
return blurred;
}
// ojo, no pinta cuando distance es mayor que 1. Testear esto!!!!!!!!!!!!!!
bool RailPen::move(int distance, Sim * sim){
Point p = getPos();
if(getMode() == PAINTING){
if(distance>0){
for(int n= 0; n<distance;n++){
Rail * newRail = makeNewRail(sim->railMap.getRailAt(p.row, p.col));
sim->railMap.setRail(p.row, p.col, newRail);
ForkRail *rf = dynamic_cast<ForkRail *>(newRail);
if(rf){
sim->addFork(rf);
}
Point lastPoint = getPos();
Dir lastDir = getLastDir();
Dir backDir = -lastDir;
lastPoint.move(backDir);
Rail * lastRail = sim->railMap.getRailAt(lastPoint.row, lastPoint.col);
if(lastRail){
newRail->linkRailAt(backDir, lastRail);
}
forward();
}
}else{
for(int n=0; n>distance;n--){
Point p = getPos();
sim->railMap.setRail(p.row, p.col, 0);
backwards();
}
}
}else{
pos.move(dir, distance);
lastDir=dir;
}
return true;
}
QString next( int k )
{
return ( k == Qt::Key_Up ) ? backwards() : forwards();
}
// fast read looper
void fastread(char *data, int x, int y, int speed)
{
bool run = false;
bool pause = false;
bool last = false;
int ch;
char *curr = data;
int length = 0;
int pivot = 0;
int maxlen = strlen(data)-1;
while(true)
{
// reset line
mvprintw(y,0," ");
clrtoeol();
char *next = curr;
next = strpbrk(next,DELIM);
// did we hit the end of the string?
if(!next || (int)(next-data)>=maxlen){
run = false;
last = true;
}
// length of the chararray (not neccessarely charachter count in utf8)
length = (int)(next-curr);
// calulate character count
int charlength = 0;
for(int i=0; i<length; i+=byteInChar((unsigned char)*(curr+i)))
++charlength;
// which letter to highlight
pivot = pivotLetter(charlength);
int pos = pivot;
// print characters before pivot letter, if any
if(pivot > 0)
{
char pre[pivot*4+1];
int offset=0;
for(int i=0; i<pivot; i++)
{
pre[i+offset]=*(curr+i+offset);
// if wide character copy the rest and alter offset
int charsize = byteInChar((unsigned char)pre[i+offset]);
for(int j=1; j<charsize; j++)
{
++offset;
pre[i+offset]=*(curr+i+offset);
}
}
// apply offset
pivot+=offset;
pre[pivot] = '\0';
mvprintw(y,x-pos,"%s",pre);
}
// print pivot letter
char mid[5];
int offset=0;
mid[0]=*(curr+pivot);
// if wide character copy the rest and alter offset
int charsize = byteInChar((unsigned char)mid[0]);
for(int i=1; i<charsize; i++)
{
++offset;
mid[i]=*(curr+pivot+offset);
}
// apply offset
pivot+=offset;
mid[1+offset] = '\0';
attron(COLOR_PAIR(1));
mvprintw(y,x,"%s",mid);
attroff(COLOR_PAIR(1));
// print characters after pivot letter, if any
if(length>pivot+1)
{
char end[length-pivot];
strncpy(end, curr+pivot+1, length-pivot-1);
end[length-pivot-1] = '\0';
mvprintw(y,x+1,"%s",end);
}
if(*next == '-')
mvprintw(y,x+length-pivot,"-");
refresh();
if(pause)
{
// pause was set -> stop run, reset pause flag
pause = false;
run = false;
}
do
{
// nonblocking keyboard input if running, blocking otherwise
if(run && !last)
timeout(0);
ch = getch();
switch(ch)
{
case ' ':
// toggle pause
if(run)
run = false;
else if(!last)
run = true;
break;
case 'q':
// quit
return;
break;
case 'k':
// less speed
speed+=50;
printSpeed(speed);
break;
case 'j':
// more speed
speed-=50;
if(speed < 50)
speed = 50;
printSpeed(speed);
break;
case 'h':
// previous word, pause
run = true;
pause = true;
last = false;
next = backwards(data, next-2, DELIM);
do
next = backwards(data, --next, DELIM);
while(strchr(DELIM,*(--next)) && (int)(next-data)>0);
break;
case 'l':
// next word, pause
run = true;
pause = true;
break;
case 'g':
// back to start
next = data;
--next;
last = false;
pause = true;
run = true;
break;
default:
break;
}
int delay = 0;
if(charlength>8)
delay = 200;
else if(charlength>12)
delay = 400;
else if(charlength>16)
delay = 600;
if(run)
usleep(1000*1000*60/speed+delay);
}
while(last || !run);
// skip multiple whitespaces until a word begins
while(next && (int)(next-data)<maxlen && strchr(DELIM,*(++next)))
next = strpbrk(next,DELIM);
// if we hit the end go back until last word begins
if((int)(next-data)>=maxlen)
{
--next;
do
next = backwards(data, next, DELIM);
while(strchr(DELIM,*(--next)) && (int)(next-data)>0);
next = backwards(data, next, DELIM);
++next;
}
curr = next;
}
}
template<typename Robot> void DDP<Robot>::iterate(int const & itr_max, std::vector<U> & us0)
{
std::vector<MatNM> Ks;
std::vector<VecN> kus;
Ks.reserve(num);
Ks.resize(num, MatNM::Zero());
kus.reserve(num);
kus.resize(num, VecN::Zero());
double lambda=params.lambda;
double dlambda=params.dlambda;
for(int i=0;i<itr_max;i++)
{
// std::cout<<"========================================================================"<<std::endl;
// std::cout<<"Iteration # "<<i<<std::endl;
// std::cout<<"------------------------------------------------------------------------"<<std::endl;
// backward pass
bool backPassDone=false;
while(!backPassDone)
{
int result=backwards(Ks,kus,lambda);
if(result>=0)
{
dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor);
lambda=std::max(lambda*dlambda, params.lambdaMin);
if(lambda>params.lambdaMax)
break;
continue;
}
backPassDone=true;
}
double gnorm=getGnorm(kus,us);
if(gnorm<params.tolGrad && lambda<1e-5)
{
dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor);
lambda=lambda*dlambda*(lambda>params.lambdaMin);
#ifdef PRINT
std::cout<<"SUCCESS: gradient norm = "<<gnorm" < tolGrad"<<std::endl
#endif
break;
}
// forward pass
bool fwdPassDone=false;
std::vector<State> xns;
std::vector<U> uns;
double Jn;
double actual;
double expected;
xns.reserve(num+1);
uns.reserve(num);
xns.resize(num+1,x0);
uns.resize(num,VecN::Zero());
if(backPassDone)
{
double alpha=params.alpha;
while(alpha>params.alphaMin)
{
forwards(Ks, kus, alpha, xns, uns, Jn);
actual=J0-Jn;
expected=-alpha*dJ(0)-alpha*alpha*dJ(1);
double reductionRatio=-1;
if(expected>0)
reductionRatio=actual/expected;
// else
// std::cout<<"WARNING: non-positive expected reduction: should not occur"<<std::endl;
if(reductionRatio>params.reductionRatioMin)
fwdPassDone=true;
break;
alpha*=params.dalphaFactor;
}
}
// std::cout<<"--------------------------------------------"<<std::endl;
// std::cout<<"Results"<<std::endl;
// std::cout<<"--------------------------------------------"<<std::endl;
if(fwdPassDone)
{
dlambda=std::min(dlambda/params.lambdaFactor, 1.0/params.lambdaFactor);
lambda=lambda*dlambda*(lambda>params.lambdaMin);
// std::cout<<"Improved"<<std::endl;
// std::cout<<"lambda: "<<lambda<<std::endl;
// std::cout<<"dlambda: "<<dlambda<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
// std::cout<<Robot::State::diff(xns[num],xrefs[num]).transpose()<<std::endl;
xs=xns;
us=uns;
J0=Jn;
if(actual<params.tolFun)
{
#ifdef PRINT
std::cout<<"SUCCESS: cost change = "<<actual<<" < tolFun"<<std::endl;
#endif
break;
}
}
else
{
dlambda=std::max(dlambda*params.lambdaFactor, params.lambdaFactor);
lambda=std::max(lambda*dlambda, params.lambdaMin);
// std::cout<<"No step found"<<std::endl;
// std::cout<<"lambda: "<<lambda<<std::endl;
// std::cout<<"dlambda: "<<dlambda<<std::endl;
// std::cout<<"Jn: "<<Jn<<std::endl;
if (lambda>params.lambdaMax)
break;
}
// std::cout<<"========================================================================"<<std::endl<<std::endl;
}
