void mouseDemo() {
mouseinitial();
_DBG_("Intialised mouse");
int sta = 0;
while(sta != 4) {
switch(sta) {
case 0:
while (coord_x<200) {
forwards(20);
}
sta = 1;
break;
case 1:
while (convertToDeg(theta) > -90) {
spinLeft();
}
sta = 2;
break;
case 2:
while (coord_y<100) {
forwards(20);
}
sta = 3;
break;
case 3:
while (convertToDeg(theta) < 90 ) {
spinRight();
}
sta = 4;
break;
}
}
}
void fullDemo() {
int robotState = 0;
uint16_t sensorPattern[5] = {0};
while(robotState != 6) {
switch(robotState) {
case 0:
while(get_coord_x() < 200) {
forwards(20);
}
robotState = 1;
break;
case 1:
setSensorSide(1);
while(get_coord_x() < 400) {
correctForwardMotion();
}
robotState = 2;
break;
case 2:
while(convertToDeg(get_theta()) > -90) {
spinLeft();
}
while(get_coord_y() < 120) {
forwards(20);
}
while(convertToDeg(get_theta()) < 0) {
spinRight();
}
while(get_coord_x() < 600) {
forwards(20);
}
robotState = 3;
break;
case 3:
setSensorSide(2);
while(get_coord_x() < 800) {
correctForwardMotion();
}
robotState = 4;
break;
case 4:
forwards(20);
while(sensorPattern[3]<2000){
getRawSensors(sensorPattern);
}
robotState = 5;
break;
case 5:
followLine();
robotState = 6;
break;
}
}
}
void playHelper(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// direction = directionPacket;
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
//m_usb_tx_string("adc1, adc2 = (");
//m_usb_tx_int(adc1);
//m_usb_tx_string(", ");
//m_usb_tx_int(adc2);
//m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
if(direction) m_usb_tx_string("direction true\n");
else m_usb_tx_string("direction false\n");
if(timer>=1){
turnLeft();
m_wait(1000);
backward();
m_wait(500);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ // PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
void playStriker(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// GET POS
getPos();
// SEND LOCATION TO HELPER
//buffer[1]=myPos[X];
//buffer[2]=myPos[Y];
//buffer[3]=direction;
//m_rf_send(TXADDRESS, buffer, PACKET_LENGTH);
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
// m_usb_tx_string("adc1, adc2 = (");
// m_usb_tx_int(adc1);
// m_usb_tx_string(", ");
// m_usb_tx_int(adc2);
// m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
// TIMER CHECKS
//m_usb_tx_string("myLastPos(x,y): (");
//m_usb_tx_int(myLastPos[X]);
//m_usb_tx_string(", ");
//m_usb_tx_int(myLastPos[Y]);
//m_usb_tx_string(") TIMER:");
//m_usb_tx_int(timer);
//m_usb_tx_string("\n");
if(timer>5 && timer<10){
backward();
m_wait(250);
turnLeft();
m_wait(250);
backward();
m_wait(250);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ //PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
void processWheels() {
if(Button(CONTROLLER_A, 5))
spinRight();
else if(Button(CONTROLLER_A, 6))
spinLeft();
if (((Happy(CONTROLLER_A, JOYSTICK_L, AXIS_X) > JOYSTICK_UPPER_THRESHOLD) || (Happy(CONTROLLER_A, JOYSTICK_L, AXIS_X) < JOYSTICK_LOWER_THRESHOLD))) {
MotorF(motor11, Happy(CONTROLLER_A, JOYSTICK_L, AXIS_X));
MotorF(motor21, Happy(CONTROLLER_A, JOYSTICK_L, AXIS_X));
}
else if (!Button(CONTROLLER_A, 5) && !Button(CONTROLLER_A, 6) && (Hat(CONTROLLER_A) == HAT_NONE)) {
MotorS(motor11);
MotorS(motor21);
}
if ((Happy(CONTROLLER_A, JOYSTICK_R, AXIS_Y) > JOYSTICK_UPPER_THRESHOLD) || (Happy(CONTROLLER_A, JOYSTICK_R, AXIS_Y) < JOYSTICK_LOWER_THRESHOLD)) {
MotorF(motor12, Happy(CONTROLLER_A, JOYSTICK_R, AXIS_Y));
MotorF(motor22, Happy(CONTROLLER_A, JOYSTICK_R, AXIS_Y));
}
else if (!Button(CONTROLLER_A, 5) && !Button(CONTROLLER_A, 6) && (Hat(CONTROLLER_A) == HAT_NONE)) {
MotorS(motor12);
MotorS(motor22);
}
if (Hat(CONTROLLER_A) == HAT_NE) {
MotorF(motor12, 90);
MotorF(motor22, 90);
MotorF(motor11, 90);
MotorF(motor21, 90);
}
else if (Hat(CONTROLLER_A) == HAT_NW) {
MotorF(motor12, 90);
MotorF(motor22, 90);
MotorF(motor11, -90);
MotorF(motor21, -90);
}
else if (Hat(CONTROLLER_A) == HAT_SW) {
MotorF(motor12, -90);
MotorF(motor22, -90);
MotorF(motor11, -90);
MotorF(motor21, -90);
}
else if (Hat(CONTROLLER_A) == HAT_SE) {
MotorF(motor12, -90);
MotorF(motor22, -90);
MotorF(motor11, 90);
MotorF(motor21, 90);
}
if (Button(CONTROLLER_A, 7)) {
MotorF(motorLED, 90);
}
else if (Button(CONTROLLER_A, 8)) {
MotorF(motorLED, -90);
}
else
MotorS(motorLED);
//if(Button(CONTROLLER_A, 9)) gyroCompensate(); //gyroTurn(45);
}
