intr_kind wizchip_getinterrupt(void)
{
uint8_t ir = 0;
uint8_t sir = 0;
uint16_t ret = 0;
#if _WIZCHIP_ == 5100
ir = getIR();
sir = ir & 0x0F;
//A20150601 : For integrating with W5300
#elif _WIZCHIP_ == 5300
ret = getIR();
ir = (uint8_t)(ret >> 8);
sir = (uint8_t)ret;
#else
ir = getIR();
sir = getSIR();
#endif
//M20150601 : For Integrating with W5300
//#if _WIZCHIP_ < 5500
#if _WIZCHIP_ < 5200
ir &= ~(1<<4); // IK_WOL
#endif
#if _WIZCHIP_ == 5200
ir &= ~(1 << 6);
#endif
ret = sir;
ret = (ret << 8) + ir;
return (intr_kind)ret;
}
char aggressiveKid(short angle, float front, float side)
{
// Returns a char if buffer distance is broken.
// If buffer not broken, it returns 'w'
// after randomly moving a short distance
getIR(); // Reads sensors
if (lt <= side) // Checks left buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'l';
}
else if (rt <= side) // Checks right buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'r';
}
else if (ft <= front) // Checks front buffer
{
SPKR_play_beep(250, 250, 100); // Beep to indicate end
return 'f';
}
else
{
// Calculate very small random angle increment
if(angle==1)
{
int angle = 21-randRange(1,40);
rotateDeg(angle); // Rotates
}
forward(move); // Moves forward
return 'w';
}
}
void wander(char type)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(1,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
if(type=='s')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = shyGuy(1,7,4,4,6);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
intr_kind wizchip_getinterrupt(void)
{
uint8_t ir = 0;
uint8_t sir = 0;
uint16_t ret = 0;
#if _WIZCHIP_ == 5100
ir = getIR();
sir = ir 0x0F;
#else
ir = getIR();
sir = getSIR();
#endif
#if _WIZCHIP_ < 5500
ir &= ~(1<<4); // IK_WOL
#endif
#if _WIZCHIP_ == 5200
ir &= ~(1 << 6);
#endif
ret = sir;
ret = (ret << 8) + ir;
return (intr_kind)ret;
}
void moveForwardSquare()
{
double xDist = 0;
double yDist = 0;
double angle = 0;
double distance = 0;
int prevTicksLeft = 0;
int prevTicksRight = 0;
drive_getTicks(&prevTicksLeft, &prevTicksRight);
while(distance < GRID_SIZE) {
getIR();
int changeVal;
if(irLeft < SENSOR_VALUE && irRight < SENSOR_VALUE){ // Wall either side
changeVal = (irLeft - irRight) * MULTIPLIER;
} else if ( irLeft < SENSOR_VALUE ) { // Wall to the left
changeVal = (irLeft - IR_LEFT) * MULTIPLIER;
} else if ( irRight < SENSOR_VALUE ) { // Wall to the right
changeVal = (IR_RIGHT - irRight) * MULTIPLIER;
} else { // If no walls to the side, carry on as normal
changeVal = 0;
}
drive_speed(BASE_SPEED_TICKS - changeVal, BASE_SPEED_TICKS + changeVal); // Set the new drive speed with the new changeVal
int ticksLeft, ticksRight;
drive_getTicks(&ticksLeft, &ticksRight); // get current ticks count
// calculate distances
double distRight = calcDistance(ticksRight, prevTicksRight); // Distance of left wheel
double distLeft = calcDistance(ticksLeft, prevTicksLeft); // Distance of right wheel
double distCentre = (distRight + distLeft) / 2; // The average of the left and right distance
angle = angle + (distRight - distLeft) / ROBOT_WIDTH;
// update prevTicks
prevTicksLeft = ticksLeft;
prevTicksRight = ticksRight;
xDist = xDist + distCentre * cos(angle);
yDist = yDist + distCentre * sin(angle);
distance = sqrt(xDist*xDist + yDist*yDist); // work out distance travelled using pythagoras
}
drive_speed(0,0);
}
void RAW_learn(struct RAWset_* RAWset) {
unsigned char cmd;
unsigned char mode;
lcd_fill(0);
draw_block(0,10,128,2,3,DRAW_PUT);
mode = 1;
while(KEY_A) {};
while(!KEY_A) {
draw_block(0,0,100,9,3,DRAW_ERASE);
set_font(BOLDFONT);
if (mode)
draw_string(0, 0, "Tasten anlernen", 3, DRAW_PUT);
else
draw_string(0, 0, "Tasten testen", 3, DRAW_PUT);
set_font(SMALLFONT);
draw_string(0, 20, "Gewuenschte Taste auf der\nBetty druecken.\nDie Tasten A, B, C und D\nkoennen nicht angelernt\nwerden", 3, DRAW_PUT);
draw_string(0, 90, "A - Exit\nB - Anlernen/Testen\n", 3, DRAW_PUT);
waitKeyUpDown();
draw_block(0,20,128,45,3,DRAW_ERASE);
draw_block(0,90,128,45,3,DRAW_ERASE);
if(KEY_A || KEY_B || KEY_C || KEY_D) {
if(KEY_B)
mode = 1-mode;
}
else {
cmd=getKeynum() -4;
if(mode) {
getIR(&(RAWset->RAWcmd[cmd]));
}
else {
RAW_Send((unsigned long)&(RAWset->RAWcmd[cmd]));
while(ANYKEY)
RAW_Repeat();
RAW_Stop();
}
}
}
}
std::vector<AXIR> AXLexer::getIRList(){
int cur = 0;
std::vector<AXIR> list;
src->seekg(header.pCS, std::ios::beg);
while(cur < header.maxCS){
int len = 0;
/* Code Position */
codePos.push_back(cur);
/* Get IR */
list.push_back(getIR(list.size(), &len));
cur += len;
/* JumpTo Queue */
for(std::vector<JumpToQueue>::iterator i = jumpToQueue.begin(); i != jumpToQueue.end();){
(*i).bytes -= len;
if((*i).bytes <= 0){
list.at((*i).jumpto).jump = list.size();
jumpToQueue.erase(i);
continue;
}
i++;
}
/* Label Queue */
for(std::vector<int>::iterator i = rawLabel.begin(); i != rawLabel.end();){
if(*i == (cur - len)){
label.push_back(list.size() - 1);
rawLabel.erase(i);
continue;
}
i++;
}
}
return list;
}
void levelZero(char type,float frontDist)
{
if(type == 'a')
{
while(1){
char robostate='w';
updateLCD();
while (robostate=='w')
{
robostate = aggressiveKid(0,6,3);
getIR();
updateLCD();
TMRSRVC_delay(100);
}
LCD_clear();
LCD_printf("Reason for Stop: %c",robostate);
if(ATTINY_get_SW_state(ATTINY_SW3))
break;
}
}
}
//proportional control
char wallFollow(char side, float min, float max, float angle)
{
getIR();
if(ft<=2.5)
{
return 'e';
}
if(side=='r')
{
// Follow right wall
if(rt < min)
{
// move away
rotateDeg(2*angle);
forward(3*move);
return 'r';
}
else if (rt > 2*max)
{
// End of wall probably
// End routine
return 'e';
}
else if (rt > max){
// move towards
rotateDeg(-angle);
forward(move);
return 'r';
}
else
{
//move forwards
forward(move);
return 'r';
}
}else if(side=='l')
{
// Follow left wall
if(lt < min){
// too close
rotateDeg(-.75*angle);
forward(move);
return 'l';
}
else if (lt > 2*max){
// end routine
return 'e';
}
else if (lt > max){
// too far
rotateDeg(angle);
forward(3*move);
return 'l';
}
else
{
forward(move);
return 'l';
}
}
else
{
// Error
SPKR_beep(440);
SPKR_beep(440);
return 'x';
}
}
char shyGuy(short angle, float front, float right, float left, float back)
{
getIR(); // Reads sensors
if (frontRight && !frontLeft)
{
// Turn left
rotateDeg(45);
return 'f';
}
else if (frontLeft && !frontRight)
{
// Turn right
rotateDeg(-45);
return 'f';
}
else if (ft <= front || (frontRight && frontLeft)) // Checks left buffer
{
float leftfront;
float rightfront;
rotateDeg(30);
getIR();
leftfront = ft;
rotateDeg(-60);
getIR();
rightfront = ft;
rotateDeg(30);
if(leftfront >= rightfront)
{
rotateDeg(90);
forward(move);
return 'f';
}
else
{
rotateDeg(-90);
forward(move);
return 'f';
}
}
else if (rt <= right)
{
rotateDeg(30);
forward(move);
return 'r';
}
else if (lt <= left)
{
rotateDeg(-30);
forward(move);
return 'l';
}else if (bk <= back)
{
forward(move);
return 'b';
}else
{
// Calculate very small random angle increment
if(angle==1)
{
int angle = 21-randRange(1,40);
rotateDeg(angle); // Rotates
}
forward(move); // Moves forward
return 'w';
}
}
char avoidObstacle(float front, float right, float left, float back){
getIR(); // Reads sensors
if (frontRight && !frontLeft)
{
// Turn left
rotateDeg(45);
return 'f';
}
else if (frontLeft && !frontRight)
{
// Turn right
rotateDeg(-45);
return 'f';
}
else if (ft <= front || (frontRight && frontLeft)) // Checks left buffer
{
float leftfront;
float rightfront;
rotateDeg(30);
getIR();
leftfront = ft;
rotateDeg(-60);
getIR();
rightfront = ft;
rotateDeg(30);
if(leftfront >= rightfront)
{
rotateDeg(90);
forward(move);
return 'f';
}
else
{
rotateDeg(-90);
forward(move);
return 'f';
}
}
else if (rt <= right)
{
rotateDeg(30);
forward(move);
return 'r';
}
else if (lt <= left)
{
rotateDeg(-30);
forward(move);
return 'l';
}else if (bk <= back)
{
forward(move);
return 'b';
}else
{
// rotate toward goal
int xdelta = xpos - roboxpos;
int ydelta = ypos - roboypos;
angle = atan2(ydelta,xdelta)*180/M_PI - roboangle;
rotateDeg(angle);
forward(.5); // Moves forward
return 'w';
}
}
