diff --git a/Chassis/Bot.h b/Chassis/Bot.h index ed07a6e..60bf492 100644 --- a/Chassis/Bot.h +++ b/Chassis/Bot.h @@ -90,7 +90,8 @@ namespace bot { rightoutput = constrain(rightoutput,1000,2000); // Invert the appropriate side - rightoutput = map(rightoutput,1000,2000,2000,1000); + // rightoutput = map(rightoutput,1000,2000,2000,1000); + leftoutput = map(leftoutput,1000,2000,2000,1000); //Send out to the motors motorLeft.writeMicroseconds(leftoutput); @@ -166,7 +167,7 @@ int maxright=0; // *10 // convert from volt to decivolt // /1024 // Divide by ADC steps to get voltage // ; - //TODO: Add board support to properly scale down 12V inputs to + //TODO: iAdd board support to properly scale down 12V inputs to return 15; } diff --git a/Chassis/Chassis.ino b/Chassis/Chassis.ino index 28bea37..19a33ca 100644 --- a/Chassis/Chassis.ino +++ b/Chassis/Chassis.ino @@ -73,9 +73,9 @@ void setup() { Scheduler.startLoop(recieve_input); //Informative tasks for debugging - // Scheduler.startLoop(print_status); + Scheduler.startLoop(print_status); // Scheduler.startLoop(printControl); - Scheduler.startLoop(printTelemetry); + // Scheduler.startLoop(printTelemetry); } bool isEnabled=false; @@ -172,7 +172,7 @@ void recieve_input(){ ){ //valid data; Handle it appropriately chassisControlData.data = radioBuffer.ccd; - // Serial.printf("[OK %2i%s] ", + // Serial.printf("\n[OK %2i%s] ", // chassisControlData.data.metadata.heartbeat, // chassisControlData.data.enable?"+":"-" // ); @@ -181,17 +181,23 @@ void recieve_input(){ //pet the watchdog to keep the system alive watchdog=0; } + else if( + radiobufferlen==CANNON_CONTROL_SIZE_BYTES && + radioBuffer.ccd.metadata.type==PacketType::CANNON_CONTROL + ){ + //Catch to avoid printing stuff + } else{ //Some other packet type //Print out info about it - // Serial.printf("?(%i) ",radiobufferlen); - // for(int i = 0; i < radiobufferlen; i++){ - // for(int j = 0; j < 8; j++){ - // Serial.print((radioBuffer.buffer[i]>>(7-j)) &1); - // } - // Serial.print("."); - // } - // Serial.println(); + Serial.printf("\n?(%i) ",radiobufferlen); + for(int i = 0; i < radiobufferlen; i++){ + for(int j = 0; j < 8; j++){ + Serial.print((radioBuffer.buffer[i]>>(7-j)) &1); + } + Serial.print("."); + } + Serial.println(); } } diff --git a/Controller/Buttons.h b/Controller/Buttons.h index bf6945f..670b7dd 100644 --- a/Controller/Buttons.h +++ b/Controller/Buttons.h @@ -33,7 +33,7 @@ namespace Buttons{ // normalizeAnalog(Buttons::leftX(),131,(543+500)/2 ,886,50) JoystickAxesConfig configLeftY={ .min=163, - .max=684, + .max=886, .idleMin=493, .idleMax=535 }; diff --git a/Controller/Controller.ino b/Controller/Controller.ino index 7bb4e78..54d95f2 100644 --- a/Controller/Controller.ino +++ b/Controller/Controller.ino @@ -5,7 +5,6 @@ #include #include #include -#include #include "Buttons.h" // #include "Pins.h" @@ -34,6 +33,12 @@ union ChassisTelemetryData { uint8_t buffer[CHASSIS_TELEMETRY_SIZE_BYTES]; } chassisTelemetryData; +union CannonControlData { + CannonControl data; + uint8_t buffer[CANNON_CONTROL_SIZE_BYTES]; +} cannonControlData; + + union RadioBuffer{ ChassisControl chassisControl; ChassisTelemetry chassisTelemetry; @@ -87,6 +92,10 @@ void setup() { Scheduler.startLoop(printChassisControl); Scheduler.startLoop(printChassisTelemetry); + Scheduler.startLoop(sendCannonControl); + // Scheduler.startLoop(printCannonControl); + + //Helpful for debugging hardware // Scheduler.startLoop(Buttons::printDebug); // Scheduler.startLoop(print_status); @@ -106,6 +115,7 @@ void loop() { /** Monitor hardware + do the things. */ void updateControls(){ + if(Buttons::home2() >1500 && chassisControlData.data.enable==false){ chassisControlData.data.enable=true; } @@ -148,6 +158,12 @@ void updateControls(){ stick.y*=-1; //Covert y from HID standard of negative-is-away to positive robot forward chassisControlData.data.speed = Chassis::arcadeDrive( stick.y, stick.x*.25 ); + + cannonControlData.data.fire=Buttons::a(); + cannonControlData.data.load=Buttons::x(); + cannonControlData.data.enable=chassisControlData.data.enable; + + delay(10); } @@ -175,6 +191,45 @@ void send_control() { delay(sleepTime.chassisControl); } +elapsedMillis cannontimer; +void sendCannonControl() { + //Observe the need of RX mode to reserve the radio; + //In such cases, just hang out until it passes control back + while (rf95.mode() == RHGenericDriver::RHModeRx) delay(1); + + cannonControlData.data.metadata.type = PacketType::CANNON_CONTROL; + cannonControlData.data.metadata.heartbeat+=1; + + bool sent = false; + sent = rf95.send(cannonControlData.buffer, CANNON_CONTROL_SIZE_BYTES); + bool done = rf95.waitPacketSent(200); + // Serial.println(); + // Serial.print("#"); + // Serial.print(sent ? ">>" : "--"); + // Serial.print(done ? "++" : "--"); + cctimer = 0; + + delay(sleepTime.chassisControl); +} + +void printCannonControl(){ + Serial.println(); + + Serial.print(" 1000){ cannonHeartbeat=0; - digitalWrite(BUILT_IN_LED, !digitalRead(BUILT_IN_LED)); + Serial.print("cannon "); + Serial.print(cannonControlData.data.metadata.heartbeat); + Serial.println(""); + + digitalWrite(BUILT_IN_LED, !digitalRead(BUILT_IN_LED) || cannonControlData.data.enable ); } //Read the encoder from the subfile @@ -190,8 +194,14 @@ void loop(){ elevationServo.writeMicroseconds(elevationMotorOutput); //Run State Machine - cannonTrigger = millis()%3000 > 1500; + // cannonTrigger = millis()%3000 > 1500; + cannonTrigger = cannonControlData.data.fire && cannonControlData.data.enable; + if(cannonControlData.data.load){ + manualReloadSwitch = ManualReloadSwitch::UNLOCKED; + }else{ + manualReloadSwitch = ManualReloadSwitch::DONE; + } run_state_machine(cannonTrigger,manualReloadSwitch); delay(10); @@ -226,15 +236,15 @@ void recieve_input(){ if (rf95.recv(radioBuffer.buffer, &radiobufferlen)) { if( - radiobufferlen==CANNON_CONTROL_SIZE_BYTES && + // radiobufferlen==CANNON_CONTROL_SIZE_BYTES && radioBuffer.ccd.metadata.type==PacketType::CANNON_CONTROL ){ //valid data; Handle it appropriately cannonControlData.data = radioBuffer.ccd; - // Serial.printf("[OK %2i%s] ", - // chassisControlData.data.metadata.heartbeat, - // chassisControlData.data.enable?"+":"-" - // ); + Serial.printf("[OK %2i%s] ", + cannonControlData.data.metadata.heartbeat, + cannonControlData.data.enable?"+":"-" + ); //pet the watchdog to keep the system alive @@ -243,19 +253,19 @@ void recieve_input(){ else{ //Some other packet type //Print out info about it - // Serial.printf("?(%i) ",radiobufferlen); - // for(int i = 0; i < radiobufferlen; i++){ - // for(int j = 0; j < 8; j++){ - // Serial.print((radioBuffer.buffer[i]>>(7-j)) &1); - // } - // Serial.print("."); - // } - // Serial.println(); + Serial.printf("?(%i) ",radiobufferlen); + for(int i = 0; i < radiobufferlen; i++){ + for(int j = 0; j < 8; j++){ + Serial.print((radioBuffer.buffer[i]>>(7-j)) &1); + } + Serial.print("."); + } + Serial.println(); } } } else { - // Serial.print("."); + Serial.print("."); } //We want this to go as fast as possible; Effectively our default task diff --git a/libraries/README.md b/libraries/README.md index 7eb00c8..858950d 100644 --- a/libraries/README.md +++ b/libraries/README.md @@ -14,8 +14,8 @@ https://learn.adafruit.com/adafruit-feather-m0-radio-with-lora-radio-module/usin Using Arduino's library manager, you'll need to install a few additional libraries -- RadioHead (a library) - Encoder by Paul Stroffregen - elapsedMillis by Paul Stroffregen - RadioHead by Mike Mccauley - Scheduler by Arduino +- PCAL6416A-IO-Expander (Not available in the arduino library - acquire via sneakernet)