Telliskivi 2 2014
Dependencies: DoubleCoilGun 4DGL-uLCD-SE ExternalIn HumanInterfaceT14 LedOut MCP3021 MODSERIAL Motor1Pwm1Dir PCA9555 PinDetect QED RgbLedPCA9555 WDT_K64F mbed-src
Fork of Telliskivi2plus by
Diff: main.cpp
- Revision:
- 5:b30229c6f32b
- Parent:
- 4:c5cf0676baca
- Child:
- 6:5619aff36840
--- a/main.cpp Mon Sep 23 14:05:38 2013 +0000 +++ b/main.cpp Sun Nov 03 11:46:19 2013 +0000 @@ -17,6 +17,8 @@ Ticker sensorUpdate; DigitalOut led1(LED1); +DigitalOut led3(LED3); +DigitalOut led4(LED4); Serial pc(USBTX, USBRX); // tx, rx //ADXL345_I2C accelerometer(p9, p10); @@ -29,11 +31,11 @@ //int gyroData[3] = {0, 0, 0}; //int16_t compassData[3] = {0, 0, 0}; -//DigitalOut led3(LED3); volatile int update = 0; volatile int extInChanged = 0; volatile int ballChanged = 0; +volatile int ballState = 0; volatile int goalButtonReleased = 0; volatile int startButtonReleased = 0; volatile int stallChanged = 0; @@ -43,8 +45,7 @@ int failSafeLimit = 60; -void executeCommand(short *cmd); -void executeCommandOld(char *buffer); +void executeCommand(char *buffer); /* void updateSensors() { @@ -70,15 +71,9 @@ Motor motor1(p22, &ioExt, 6, 7, p14, p15); //FL - M4 Motor motor3(p21, &ioExt, 9, 8, p16, p17); //RL - M5 -//LedOut redLed(&ioExt, 13); -//LedOut blueLed(&ioExt, 12); -//LedOut yellowLed(&ioExt, 11); +Coilgun coilgun(p20, p19, p18); -Coilgun coilgun(p20, p19, p18); -//CoilGun coilgun(LED3, LED4, p18); -//Timeout kickFinish; - -HumanInterface humanInterface(&pc, &ioExt, 11, 12, 13, 14, 15, 10); +HumanInterface humanInterface(&ioExt, 15, 14, 13, 12, 11, p29, p30, p26); void updateTick() { //led3 = 1; @@ -95,8 +90,14 @@ extInChanged = 1; } -void ballChangedCallback() { +void ballRiseCallback() { ballChanged = 1; + ballState = 1; +} + +void ballFallCallback() { + ballChanged = 1; + ballState = 0; } void goalRiseCallback() { @@ -121,8 +122,6 @@ char sendBuffer[256]; int charCounter = 0; -DigitalOut led2(LED2); - //void readSerial() { //int n = pc.readable(); @@ -133,7 +132,6 @@ //pc.scanf("%s", &buffer); //pc.printf("%c\n", pc.getc()); //buffer[charCounter] = pc.getc(); - //led2 = !led2; /*printf("%c\n", buffer[charCounter]); //pc.printf("%s\n", buffer); if (buffer[charCounter] == '\n') { @@ -209,21 +207,24 @@ //int ioExtBefore = ioExt.read(); //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read())); - pc.printf("ioExt: %x\n", ioExt.read()); - ioExt.setDirection(0xC400); + //pc.printf("ioExt: %x\n", ioExt.read()); + ioExt.setDirection(0x0400); ioExt.write(0x0155); //int ioExtAfter = ioExt.read(); - pc.printf("ioExt: %x\n", ioExt.read()); + //pc.printf("ioExt: %x\n", ioExt.read()); //pc.printf("ioExt: %s\n", byte_to_binary(ioExt.read())); //ballSens.change(&extChangedCallback); //button1.change(&extChangedCallback); //button2.change(&extChangedCallback); - //humanInterface.change(&extChangedCallback); - humanInterface.ballChange(&ballChangedCallback); + + humanInterface.ballRise(&ballRiseCallback); + humanInterface.ballFall(&ballFallCallback); humanInterface.goalRise(&goalRiseCallback); humanInterface.startRise(&startRiseCallback); + //humanInterface.change(&extChangedCallback); + motor1.stallChange(&stallChangedCallback); motor2.stallChange(&stallChangedCallback); motor3.stallChange(&stallChangedCallback); @@ -287,40 +288,33 @@ //server.sendTo(client, sendBuffer, charCount); //pc.printf("<speeds:%d:%d:%d:%d:%d>\n", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); //redLed.toggle(); + + ioExt.setDirection(0x0400); + led1 = !led1; } if (stallChanged) { stallChanged = 0; - int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>\n", + int charCount = sprintf(sendBuffer, "<stall:%d:%d:%d:%d:%d>", motor1.getStallLevel(), motor2.getStallLevel(), motor3.getStallLevel(), motor4.getStallLevel(), motor5.getStallLevel()); server.sendTo(client, sendBuffer, charCount); - } + } - /*if (extInChanged) { - extInChanged = 0; - pc.printf("extChanged\n"); - //bool ballState = ballSens.read(); - //bool button1State = button1.read(); - //bool button2State = button2.read(); - if (button1State) { - led1 = 1; - } else { - led1 = 0; - } - pc.printf("<ball:%d>\n", ballState ? 1 : 0); - pc.printf("<btn1:%d>\n", button1State ? 1 : 0); - pc.printf("<btn2:%d>\n", button2State ? 1 : 0); - }*/ if (ballChanged) { ballChanged = 0; - int charCount = sprintf(sendBuffer, "<ball:%d>", humanInterface.getBallState()); + int charCount = sprintf(sendBuffer, "<ball:%d>", ballState); server.sendTo(client, sendBuffer, charCount); + if (ballState) { + led4 = 1; + } else { + led4 = 0; + } } if (goalButtonReleased) { @@ -332,6 +326,13 @@ startButtonReleased = 0; server.sendTo(client, "<toggle-go>", 11); } + + /*if (extInChanged) { + extInChanged = 0; + bool ballState = humanInterface.getBallState(); + int charCount = sprintf(sendBuffer, "<ioExt:%x>", ioExt.getLastRead()); + server.sendTo(client, sendBuffer, charCount); + }*/ //printf("\nWait for packet...\n"); @@ -342,7 +343,7 @@ //pc.printf("n: %d\n", n); buffer[n] = '\0'; //server.sendTo(client, buffer, n); - executeCommandOld(buffer); + executeCommand(buffer); //coilgun.kick(1000); //executeCommand((short*)buffer); } @@ -370,33 +371,7 @@ } } -void executeCommand(short *cmd) { - if (cmd[0] == 1) { - motor1.setSpeed(cmd[1]); - motor2.setSpeed(cmd[2]); - motor3.setSpeed(cmd[3]); - motor4.setSpeed(cmd[4]); - motor5.setSpeed(cmd[5]); - } else if (cmd[0] == 2) { - int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); - //pc.printf("%d\n", sizeof(sendBuffer)); - //pc.printf("sendTo %s\n", client.get_address()); - server.sendTo(client, sendBuffer, charCount); - led2 = !led2; - } else if (cmd[0] == 3) { - pc.printf("kick\n"); - coilgun.kick(cmd[1]); - } else if (cmd[0] == 4) { - pc.printf("charge\n"); - if (cmd[1] == 1) { - coilgun.charge(); - } else { - coilgun.chargeEnd(); - } - } -} - -void executeCommandOld(char *buffer) { +void executeCommand(char *buffer) { failSafeCount = 0; pc.printf("%s\n", buffer); @@ -418,7 +393,6 @@ int charCount = sprintf(sendBuffer, "<speeds:%d:%d:%d:%d:%d>", motor1.getSpeed(), motor2.getSpeed(), motor3.getSpeed(), motor4.getSpeed(), motor5.getSpeed()); server.sendTo(client, sendBuffer, charCount); - led2 = !led2; } else if (strncmp(cmd, "kick", 4) == 0) { pc.printf("kick\n"); coilgun.kick(atoi(strtok(NULL, ":"))); @@ -450,6 +424,7 @@ motor5.setSpeed(0); humanInterface.setGoal(HumanInterface::UNSET); humanInterface.setError(false); + humanInterface.setGo(false); } else if (strncmp(cmd, "fs", 2) == 0) { failSafeEnabled = (bool)atoi(strtok(NULL, ":")); } else if (strncmp(cmd, "target", 6) == 0) { @@ -463,5 +438,7 @@ } } else if (strncmp(cmd, "error", 5) == 0) { humanInterface.setError(atoi(strtok(NULL, ":"))); + } else if (strncmp(cmd, "go", 2) == 0) { + humanInterface.setGo(atoi(strtok(NULL, ":"))); } } \ No newline at end of file