├── src └── thirdparty │ └── RoboClaw │ ├── examples │ ├── PacketSerialReadVersion │ │ └── PacketSerialReadVersion.ino │ ├── SimpleSerial │ │ └── SimpleSerial.ino │ ├── RCpulseExample │ │ └── RCpulseExample.ino │ ├── PacketSerialMixedPWM │ │ └── PacketSerialMixedPWM.ino │ ├── PacketSerialSimplePWM │ │ └── PacketSerialSimplePWM.ino │ ├── PacketSerialEncoderPosition │ │ ├── PacketSerialEncoderPosition.ino.bak │ │ └── PacketSerialEncoderPosition.ino │ ├── RCpulseMixedExample │ │ └── RCpulseMixedExample.ino │ ├── PacketSerialEncoderRead │ │ └── PacketSerialEncoderRead.ino │ ├── PacketSerialEncoderSpeed │ │ └── PacketSerialEncoderSpeed.ino │ ├── PacketSerialEncoderSpeedAccel │ │ └── PacketSerialEncoderSpeedAccel.ino │ ├── PacketSerialEncoderSpeedAccelDistance │ │ └── PacketSerialEncoderSpeedAccelDistance.ino │ ├── PacketSerialEncoderSpeedDistance │ │ └── PacketSerialEncoderSpeedDistance.ino │ └── PacketSerialEncoderBareMinimum │ │ └── PacketSerialEncoderBareMinimum.ino │ ├── keywords.txt │ ├── RoboClaw.h │ └── RoboClaw.cpp ├── .github └── workflows │ └── build.yaml ├── pitches.h ├── Buttons.cpp ├── Pressure.h ├── Buttons.h ├── cpp_utils.h ├── README.md ├── Utilities.h ├── Input.cpp ├── Utilities.cpp ├── Input.h ├── Logging.h ├── Constants.h ├── Logging.cpp ├── Alarms.cpp ├── Display.cpp ├── Display.h ├── Alarms.h └── e-vent.ino /src/thirdparty/RoboClaw/examples/PacketSerialReadVersion/PacketSerialReadVersion.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | void setup() { 14 | //Open Serial and roboclaw serial ports 15 | Serial.begin(57600); 16 | rc.begin(38400); 17 | } 18 | 19 | void loop() { 20 | char version[32]; 21 | 22 | if(rc.ReadVersion(address,version)){ 23 | Serial.println(version); 24 | } 25 | 26 | delay(100); 27 | } 28 | 29 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/SimpleSerial/SimpleSerial.ino: -------------------------------------------------------------------------------- 1 | //Roboclaw simple serial example. Set mode to 6. Option to 4(38400 bps) 2 | #include 3 | 4 | //See limitations of Arduino SoftwareSerial 5 | 6 | SoftwareSerial mySerial(10,11); 7 | 8 | void setup() { 9 | mySerial.begin(38400); 10 | } 11 | 12 | void loop() { 13 | mySerial.write(1); 14 | mySerial.write(-127); 15 | delay(2000); 16 | mySerial.write(64); 17 | delay(1000); 18 | mySerial.write(127); 19 | mySerial.write(-1); 20 | delay(2000); 21 | mySerial.write(-64); 22 | delay(1000); 23 | mySerial.write(1); 24 | mySerial.write(-1); 25 | delay(2000); 26 | mySerial.write(0); 27 | delay(1000); 28 | mySerial.write(127); 29 | mySerial.write(-127); 30 | delay(2000); 31 | mySerial.write(0); 32 | delay(1000); 33 | } 34 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/RCpulseExample/RCpulseExample.ino: -------------------------------------------------------------------------------- 1 | //On Roboclaw set switch 1 and 6 on. 2 | 3 | #include 4 | 5 | Servo myservo1; // create servo object to control a Roboclaw channel 6 | Servo myservo2; // create servo object to control a Roboclaw channel 7 | 8 | int pos = 0; // variable to store the servo position 9 | 10 | void setup() 11 | { 12 | myservo1.attach(5); // attaches the RC signal on pin 5 to the servo object 13 | myservo2.attach(6); // attaches the RC signal on pin 6 to the servo object 14 | } 15 | 16 | void loop() 17 | { 18 | myservo1.writeMicroseconds(1250); //full forward 19 | myservo2.writeMicroseconds(1750); //full reverse 20 | delay(2000); 21 | myservo1.writeMicroseconds(1750); //full reverse 22 | myservo2.writeMicroseconds(1250); //full forward 23 | delay(2000); 24 | } 25 | -------------------------------------------------------------------------------- /.github/workflows/build.yaml: -------------------------------------------------------------------------------- 1 | # based on https://blog.arduino.cc/2019/11/14/arduino-on-github-actions/ 2 | 3 | name: build 4 | 5 | on: [push, pull_request] 6 | 7 | jobs: 8 | 9 | build-matrix: 10 | 11 | strategy: 12 | 13 | matrix: 14 | os: [windows-latest, ubuntu-latest] 15 | 16 | arduino-platform: ["arduino:avr"] 17 | 18 | include: 19 | - arduino-platform: "arduino:avr" 20 | fqbn: "arduino:avr:mega:cpu=atmega2560" 21 | 22 | runs-on: ${{ matrix.os }} 23 | 24 | steps: 25 | - name: Checkout 26 | uses: actions/checkout@master 27 | 28 | - name: Setup Arduino CLI 29 | uses: arduino/setup-arduino-cli@v1.0.0 30 | 31 | - name: Install platform 32 | run: | 33 | arduino-cli core update-index 34 | arduino-cli core install ${{ matrix.arduino-platform }} 35 | arduino-cli lib install SD LiquidCrystal 36 | 37 | - name: Compile Sketch 38 | run: arduino-cli compile --fqbn ${{ matrix.fqbn }} ./e-vent.ino 39 | 40 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialMixedPWM/PacketSerialMixedPWM.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | void setup() { 14 | //Communciate with roboclaw at 38400bps 15 | roboclaw.begin(38400); 16 | roboclaw.ForwardMixed(address, 0); 17 | roboclaw.TurnRightMixed(address, 0); 18 | } 19 | 20 | void loop() { 21 | roboclaw.ForwardMixed(address, 64); 22 | delay(2000); 23 | roboclaw.BackwardMixed(address, 64); 24 | delay(2000); 25 | roboclaw.TurnRightMixed(address, 64); 26 | delay(2000); 27 | roboclaw.TurnLeftMixed(address, 64); 28 | delay(2000); 29 | roboclaw.ForwardMixed(address, 0); 30 | roboclaw.TurnRightMixed(address, 64); 31 | delay(2000); 32 | roboclaw.TurnLeftMixed(address, 64); 33 | delay(2000); 34 | roboclaw.TurnRightMixed(address, 0); 35 | delay(2000); 36 | } 37 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialSimplePWM/PacketSerialSimplePWM.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | void setup() { 14 | //Open roboclaw serial ports 15 | roboclaw.begin(38400); 16 | } 17 | 18 | void loop() { 19 | roboclaw.ForwardM1(address,64); //start Motor1 forward at half speed 20 | roboclaw.BackwardM2(address,64); //start Motor2 backward at half speed 21 | delay(2000); 22 | 23 | roboclaw.BackwardM1(address,64); 24 | roboclaw.ForwardM2(address,64); 25 | delay(2000); 26 | 27 | roboclaw.ForwardBackwardM1(address,96); //start Motor1 forward at half speed 28 | roboclaw.ForwardBackwardM2(address,32); //start Motor2 backward at half speed 29 | delay(2000); 30 | 31 | roboclaw.ForwardBackwardM1(address,32); 32 | roboclaw.ForwardBackwardM2(address,96); 33 | delay(2000); 34 | } 35 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderPosition/PacketSerialEncoderPosition.ino.bak: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Display Encoder and Speed for Motor 1 14 | void displayspeed(void) 15 | { 16 | uint8_t status1,status2; 17 | bool valid1,valid2; 18 | int32_t enc1 = roboclaw.ReadEncM1(address, &status1, &valid1); 19 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status2, &valid2); 20 | 21 | if(valid1){ 22 | Serial.print("Encoder1:"); 23 | Serial.print(enc1,DEC); 24 | Serial.print(" "); 25 | Serial.print(status1,HEX); 26 | Serial.print(" "); 27 | } 28 | if(valid2){ 29 | Serial.print("Speed1:"); 30 | Serial.print(speed1,DEC); 31 | Serial.print(" "); 32 | } 33 | 34 | Serial.println(); 35 | } 36 | 37 | //This is the first function arduino runs on reset/power up 38 | void setup() { 39 | //Open Serial and roboclaw at 38400bps 40 | Serial.begin(57600); 41 | roboclaw.begin(38400); 42 | 43 | Serial.println("Starting..."); 44 | } 45 | 46 | void loop() { 47 | roboclaw.SpeedAccelDeccelPositionM1(address,0,12000,0,11000,1); 48 | roboclaw.SpeedAccelDeccelPositionM1(address,0,12000,0,1000,0); 49 | roboclaw.SpeedAccelDeccelPositionM1(address,32000,12000,32000,11000,0); 50 | roboclaw.SpeedAccelDeccelPositionM1(address,32000,12000,32000,1000,0); 51 | long last = millis(); 52 | while(millis()-last<5000){ 53 | displayspeed(); 54 | delay(50); 55 | } 56 | } -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderPosition/PacketSerialEncoderPosition.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Display Encoder and Speed for Motor 1 14 | void displayspeed(void) 15 | { 16 | uint8_t status1,status2; 17 | bool valid1,valid2; 18 | int32_t enc1 = roboclaw.ReadEncM1(address, &status1, &valid1); 19 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status2, &valid2); 20 | 21 | if(valid1){ 22 | Serial.print("Encoder1:"); 23 | Serial.print(enc1,DEC); 24 | Serial.print(" "); 25 | Serial.print(status1,HEX); 26 | Serial.print(" "); 27 | } 28 | if(valid2){ 29 | Serial.print("Speed1:"); 30 | Serial.print(speed1,DEC); 31 | Serial.print(" "); 32 | } 33 | 34 | Serial.println(); 35 | } 36 | 37 | //This is the first function arduino runs on reset/power up 38 | void setup() { 39 | //Open Serial and roboclaw at 38400bps 40 | Serial.begin(57600); 41 | roboclaw.begin(38400); 42 | 43 | Serial.println("Starting..."); 44 | } 45 | 46 | void loop() { 47 | roboclaw.SpeedAccelDeccelPositionM1(address,0,12000,0,11000,1); 48 | roboclaw.SpeedAccelDeccelPositionM1(address,0,12000,0,1000,0); 49 | roboclaw.SpeedAccelDeccelPositionM1(address,32000,12000,32000,11000,0); 50 | roboclaw.SpeedAccelDeccelPositionM1(address,32000,12000,32000,1000,0); 51 | long last = millis(); 52 | while(millis()-last<5000){ 53 | displayspeed(); 54 | delay(50); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/RCpulseMixedExample/RCpulseMixedExample.ino: -------------------------------------------------------------------------------- 1 | //On Roboclaw set switch 1 and 6 on. // <-- what does this refer to? 2 | //mode 2 option 4 // <-- my note based on user manual pg 26 3 | 4 | #include 5 | 6 | Servo myservo1; // create servo object to control a Roboclaw channel 7 | Servo myservo2; // create servo object to control a Roboclaw channel 8 | 9 | //int pos = 0; // variable to store the servo position //<-- left-over from arduino ide servo sweep example? 10 | 11 | void setup() 12 | { 13 | myservo1.attach(5); // attaches the RC signal on pin 5 to the servo object 14 | myservo2.attach(6); // attaches the RC signal on pin 6 to the servo object 15 | } 16 | 17 | 18 | void loop() 19 | { 20 | //forward 21 | myservo1.writeMicroseconds(1600); 22 | myservo2.writeMicroseconds(1500); 23 | delay(2000); 24 | 25 | //backward 26 | myservo1.writeMicroseconds(1400); 27 | myservo2.writeMicroseconds(1500); 28 | delay(2000); 29 | 30 | //left 31 | myservo1.writeMicroseconds(1500); 32 | myservo2.writeMicroseconds(1600); 33 | delay(2000); 34 | 35 | //right 36 | myservo1.writeMicroseconds(1500); 37 | myservo2.writeMicroseconds(1400); 38 | delay(2000); 39 | 40 | //mixed forward/left 41 | myservo1.writeMicroseconds(1600); 42 | myservo2.writeMicroseconds(1600); 43 | delay(2000); 44 | 45 | //mixed forward/right 46 | myservo1.writeMicroseconds(1600); 47 | myservo2.writeMicroseconds(1400); 48 | delay(2000); 49 | 50 | //mixed backward/left 51 | myservo1.writeMicroseconds(1400); 52 | myservo2.writeMicroseconds(1600); 53 | delay(2000); 54 | 55 | //mixed backward/right 56 | myservo1.writeMicroseconds(1400); 57 | myservo2.writeMicroseconds(1400); 58 | delay(2000); 59 | 60 | } 61 | 62 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderRead/PacketSerialEncoderRead.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | void setup() { 14 | //Open Serial and roboclaw at 38400bps 15 | Serial.begin(57600); 16 | roboclaw.begin(38400); 17 | } 18 | 19 | void loop() { 20 | uint8_t status1,status2,status3,status4; 21 | bool valid1,valid2,valid3,valid4; 22 | 23 | //Read all the data from Roboclaw before displaying on Serial Monitor window 24 | //This prevents the hardware serial interrupt from interfering with 25 | //reading data using software serial. 26 | int32_t enc1= roboclaw.ReadEncM1(address, &status1, &valid1); 27 | int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2); 28 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3); 29 | int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4); 30 | 31 | Serial.print("Encoder1:"); 32 | if(valid1){ 33 | Serial.print(enc1,HEX); 34 | Serial.print(" "); 35 | Serial.print(status1,HEX); 36 | Serial.print(" "); 37 | } 38 | else{ 39 | Serial.print("invalid "); 40 | } 41 | Serial.print("Encoder2:"); 42 | if(valid2){ 43 | Serial.print(enc2,HEX); 44 | Serial.print(" "); 45 | Serial.print(status2,HEX); 46 | Serial.print(" "); 47 | } 48 | else{ 49 | Serial.print("invalid "); 50 | } 51 | Serial.print("Speed1:"); 52 | if(valid3){ 53 | Serial.print(speed1,HEX); 54 | Serial.print(" "); 55 | } 56 | else{ 57 | Serial.print("invalid "); 58 | } 59 | Serial.print("Speed2:"); 60 | if(valid4){ 61 | Serial.print(speed2,HEX); 62 | Serial.print(" "); 63 | } 64 | else{ 65 | Serial.print("invalid "); 66 | } 67 | Serial.println(); 68 | 69 | delay(100); 70 | } 71 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderSpeed/PacketSerialEncoderSpeed.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Velocity PID coefficients. 14 | #define Kp 1.0 15 | #define Ki 0.5 16 | #define Kd 0.25 17 | #define qpps 44000 18 | 19 | void setup() { 20 | //Open Serial and roboclaw serial ports 21 | Serial.begin(57600); 22 | roboclaw.begin(38400); 23 | 24 | //Set PID Coefficients 25 | roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); 26 | roboclaw.SetM2VelocityPID(address,Kd,Kp,Ki,qpps); 27 | } 28 | 29 | void displayspeed(void) 30 | { 31 | uint8_t status1,status2,status3,status4; 32 | bool valid1,valid2,valid3,valid4; 33 | 34 | int32_t enc1= roboclaw.ReadEncM1(address, &status1, &valid1); 35 | int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2); 36 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3); 37 | int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4); 38 | Serial.print("Encoder1:"); 39 | if(valid1){ 40 | Serial.print(enc1,HEX); 41 | Serial.print(" "); 42 | Serial.print(status1,HEX); 43 | Serial.print(" "); 44 | } 45 | else{ 46 | Serial.print("invalid "); 47 | } 48 | Serial.print("Encoder2:"); 49 | if(valid2){ 50 | Serial.print(enc2,HEX); 51 | Serial.print(" "); 52 | Serial.print(status2,HEX); 53 | Serial.print(" "); 54 | } 55 | else{ 56 | Serial.print("invalid "); 57 | } 58 | Serial.print("Speed1:"); 59 | if(valid3){ 60 | Serial.print(speed1,HEX); 61 | Serial.print(" "); 62 | } 63 | else{ 64 | Serial.print("invalid "); 65 | } 66 | Serial.print("Speed2:"); 67 | if(valid4){ 68 | Serial.print(speed2,HEX); 69 | Serial.print(" "); 70 | } 71 | else{ 72 | Serial.print("invalid "); 73 | } 74 | Serial.println(); 75 | } 76 | 77 | void loop() { 78 | roboclaw.SpeedM1(address,12000); 79 | for(uint8_t i = 0;i<100;i++){ 80 | displayspeed(); 81 | delay(10); 82 | } 83 | 84 | roboclaw.SpeedM1(address,-12000); 85 | for(uint8_t i = 0;i<100;i++){ 86 | displayspeed(); 87 | delay(10); 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /pitches.h: -------------------------------------------------------------------------------- 1 | /************************************************* 2 | * Public Constants 3 | *************************************************/ 4 | 5 | #define NOTE_B0 31 6 | #define NOTE_C1 33 7 | #define NOTE_CS1 35 8 | #define NOTE_D1 37 9 | #define NOTE_DS1 39 10 | #define NOTE_E1 41 11 | #define NOTE_F1 44 12 | #define NOTE_FS1 46 13 | #define NOTE_G1 49 14 | #define NOTE_GS1 52 15 | #define NOTE_A1 55 16 | #define NOTE_AS1 58 17 | #define NOTE_B1 62 18 | #define NOTE_C2 65 19 | #define NOTE_CS2 69 20 | #define NOTE_D2 73 21 | #define NOTE_DS2 78 22 | #define NOTE_E2 82 23 | #define NOTE_F2 87 24 | #define NOTE_FS2 93 25 | #define NOTE_G2 98 26 | #define NOTE_GS2 104 27 | #define NOTE_A2 110 28 | #define NOTE_AS2 117 29 | #define NOTE_B2 123 30 | #define NOTE_C3 131 31 | #define NOTE_CS3 139 32 | #define NOTE_D3 147 33 | #define NOTE_DS3 156 34 | #define NOTE_E3 165 35 | #define NOTE_F3 175 36 | #define NOTE_FS3 185 37 | #define NOTE_G3 196 38 | #define NOTE_GS3 208 39 | #define NOTE_A3 220 40 | #define NOTE_AS3 233 41 | #define NOTE_B3 247 42 | #define NOTE_C4 262 43 | #define NOTE_CS4 277 44 | #define NOTE_D4 294 45 | #define NOTE_DS4 311 46 | #define NOTE_E4 330 47 | #define NOTE_F4 349 48 | #define NOTE_FS4 370 49 | #define NOTE_G4 392 50 | #define NOTE_GS4 415 51 | #define NOTE_A4 440 52 | #define NOTE_AS4 466 53 | #define NOTE_B4 494 54 | #define NOTE_C5 523 55 | #define NOTE_CS5 554 56 | #define NOTE_D5 587 57 | #define NOTE_DS5 622 58 | #define NOTE_E5 659 59 | #define NOTE_F5 698 60 | #define NOTE_FS5 740 61 | #define NOTE_G5 784 62 | #define NOTE_GS5 831 63 | #define NOTE_A5 880 64 | #define NOTE_AS5 932 65 | #define NOTE_B5 988 66 | #define NOTE_C6 1047 67 | #define NOTE_CS6 1109 68 | #define NOTE_D6 1175 69 | #define NOTE_DS6 1245 70 | #define NOTE_E6 1319 71 | #define NOTE_F6 1397 72 | #define NOTE_FS6 1480 73 | #define NOTE_G6 1568 74 | #define NOTE_GS6 1661 75 | #define NOTE_A6 1760 76 | #define NOTE_AS6 1865 77 | #define NOTE_B6 1976 78 | #define NOTE_C7 2093 79 | #define NOTE_CS7 2217 80 | #define NOTE_D7 2349 81 | #define NOTE_DS7 2489 82 | #define NOTE_E7 2637 83 | #define NOTE_F7 2794 84 | #define NOTE_FS7 2960 85 | #define NOTE_G7 3136 86 | #define NOTE_GS7 3322 87 | #define NOTE_A7 3520 88 | #define NOTE_AS7 3729 89 | #define NOTE_B7 3951 90 | #define NOTE_C8 4186 91 | #define NOTE_CS8 4435 92 | #define NOTE_D8 4699 93 | #define NOTE_DS8 4978 94 | -------------------------------------------------------------------------------- /Buttons.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Buttons.cpp 29 | */ 30 | 31 | #include "Buttons.h" 32 | 33 | 34 | namespace buttons { 35 | 36 | 37 | /// PressHoldButton /// 38 | 39 | void PressHoldButton::update() { 40 | const unsigned long time_now = millis(); 41 | const unsigned long time_since_last_low = time_now - last_low_time_; 42 | const bool press_lost = time_since_last_low > kDebounceDelay; 43 | if (digitalRead(pin_) == LOW) { 44 | if (!press_lost) { 45 | current_hold_time_ += time_since_last_low; 46 | } 47 | last_low_time_ = time_now; 48 | } 49 | else if (press_lost) { 50 | current_hold_time_ = 0; 51 | } 52 | } 53 | 54 | 55 | /// DebouncedButton /// 56 | 57 | DebouncedButton::DebouncedButton(const int& pin): pin_(pin) {} 58 | 59 | void DebouncedButton::begin() { 60 | pinMode(pin_, INPUT_PULLUP); 61 | } 62 | 63 | bool DebouncedButton::is_LOW() { 64 | int reading = digitalRead(pin_); 65 | 66 | bool low_value = false; 67 | const unsigned long time_now = millis(); 68 | if (reading == LOW) { 69 | if ((time_now - last_low_time_) > kDebounceDelay) { 70 | low_value = true; 71 | } 72 | last_low_time_ = time_now; 73 | } 74 | return low_value; 75 | } 76 | 77 | 78 | } // namespace buttons 79 | 80 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderSpeedAccel/PacketSerialEncoderSpeedAccel.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Velocity PID coefficients 14 | #define Kp 1.0 15 | #define Ki 0.5 16 | #define Kd 0.25 17 | #define qpps 44000 18 | 19 | void setup() { 20 | //Open Serial and roboclaw serial ports 21 | Serial.begin(57600); 22 | roboclaw.begin(38400); 23 | 24 | //Set PID Coefficients 25 | roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); 26 | roboclaw.SetM2VelocityPID(address,Kd,Kp,Ki,qpps); 27 | } 28 | 29 | void displayspeed(void) 30 | { 31 | uint8_t status1,status2,status3,status4; 32 | bool valid1,valid2,valid3,valid4; 33 | 34 | int32_t enc1= roboclaw.ReadEncM1(address, &status1, &valid1); 35 | int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2); 36 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3); 37 | int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4); 38 | Serial.print("Encoder1:"); 39 | if(valid1){ 40 | Serial.print(enc1,HEX); 41 | Serial.print(" "); 42 | Serial.print(status1,HEX); 43 | Serial.print(" "); 44 | } 45 | else{ 46 | Serial.print("invalid "); 47 | } 48 | Serial.print("Encoder2:"); 49 | if(valid2){ 50 | Serial.print(enc2,HEX); 51 | Serial.print(" "); 52 | Serial.print(status2,HEX); 53 | Serial.print(" "); 54 | } 55 | else{ 56 | Serial.print("invalid "); 57 | } 58 | Serial.print("Speed1:"); 59 | if(valid3){ 60 | Serial.print(speed1,HEX); 61 | Serial.print(" "); 62 | } 63 | else{ 64 | Serial.print("invalid "); 65 | } 66 | Serial.print("Speed2:"); 67 | if(valid4){ 68 | Serial.print(speed2,HEX); 69 | Serial.print(" "); 70 | } 71 | else{ 72 | Serial.print("invalid "); 73 | } 74 | Serial.println(); 75 | } 76 | 77 | void loop() { 78 | roboclaw.SpeedAccelM1(address,12000,12000); 79 | roboclaw.SpeedAccelM2(address,12000,-12000); 80 | for(uint8_t i = 0;i<100;i++){ 81 | displayspeed(); 82 | delay(10); 83 | } 84 | 85 | roboclaw.SpeedAccelM1(address,12000,-12000); 86 | roboclaw.SpeedAccelM2(address,12000,12000); 87 | for(uint8_t i = 0;i<100;i++){ 88 | displayspeed(); 89 | delay(10); 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /Pressure.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Pressure.h 29 | * Calculates and stores the key pressure values of the breathing cycle. 30 | */ 31 | 32 | #ifndef Pressure_h 33 | #define Pressure_h 34 | 35 | #include "Arduino.h" 36 | 37 | class Pressure { 38 | public: 39 | Pressure(int pin): 40 | sense_pin_(pin), 41 | current_(0.0), 42 | current_peak_(0.0), 43 | peak_(0.0), 44 | plateau_(0.0), 45 | peep_(0.0) {} 46 | 47 | //Get pressure reading 48 | void read() { 49 | // read the voltage 50 | int V = analogRead(sense_pin_); 51 | 52 | float Pmin = -100.0; // pressure max in mbar 53 | float Pmax = 100.0; // pressure min in mbar 54 | float Vmax = 1024; // max voltage in range from analogRead 55 | // convert to pressure 56 | float pres = (10 * V/Vmax - 1) * (Pmax-Pmin)/8. + Pmin; //mmHg 57 | 58 | // convert to cmH20 59 | pres *= 1.01972; 60 | 61 | // update peak 62 | current_peak_ = max(current_peak_, pres); 63 | 64 | current_ = pres; 65 | } 66 | 67 | const float& get() { 68 | return current_; 69 | } 70 | 71 | void set_peak_and_reset() { 72 | peak_ = current_peak_; 73 | current_peak_ = 0.0; 74 | } 75 | 76 | void set_plateau() { 77 | plateau_ = get(); 78 | } 79 | 80 | void set_peep() { 81 | peep_ = get(); 82 | } 83 | 84 | const float& peak() { return peak_; } 85 | const float& plateau() { return plateau_; } 86 | const float& peep() { return peep_; } 87 | 88 | private: 89 | int sense_pin_; 90 | float current_; 91 | float current_peak_; 92 | float peak_, plateau_, peep_; 93 | }; 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /Buttons.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Buttons.h 29 | * Defines methods for quickly and reliably checking different button actions 30 | * such as pressing and pressing and holding. 31 | */ 32 | 33 | #ifndef Buttons_h 34 | #define Buttons_h 35 | 36 | #include "Arduino.h" 37 | 38 | 39 | namespace buttons { 40 | 41 | 42 | /** 43 | * PressHoldButton 44 | * Button abstraction capable of detecting if a given button has been held 45 | * pressed for a given ammount of time. 46 | */ 47 | class PressHoldButton { 48 | 49 | const unsigned long kDebounceDelay = 100; 50 | 51 | public: 52 | PressHoldButton(const int& pin, const unsigned long& hold_duration): 53 | pin_(pin), 54 | hold_duration_(hold_duration) {} 55 | 56 | // Setup during arduino setup() 57 | inline void begin() { pinMode(pin_, INPUT_PULLUP); } 58 | 59 | // Update during arduino loop() 60 | void update(); 61 | 62 | // Check if button was just held for hold_duration ms. 63 | inline bool wasHeld() { return current_hold_time_ > hold_duration_; } 64 | 65 | private: 66 | int pin_; 67 | unsigned long hold_duration_; 68 | unsigned long last_low_time_ = 0; 69 | unsigned long current_hold_time_ = 0; 70 | }; 71 | 72 | 73 | /** 74 | * DebouncedButton 75 | * Represents a pullup button that filters out unintended LOW readings. 76 | */ 77 | class DebouncedButton { 78 | 79 | const unsigned long kDebounceDelay = 100; 80 | 81 | public: 82 | DebouncedButton(const int& pin); 83 | 84 | // Setup during arduino setup() 85 | void begin(); 86 | 87 | // Check if button is low 88 | bool is_LOW(); 89 | 90 | private: 91 | int pin_; 92 | unsigned long last_low_time_ = 0; 93 | }; 94 | 95 | 96 | } // namespace buttons 97 | 98 | 99 | #endif 100 | 101 | -------------------------------------------------------------------------------- /cpp_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef CPP_UTILS_H_INCLUDED 2 | #define CPP_UTILS_H_INCLUDED 3 | 4 | /* 5 | * As far as this file is located with parts of GCC C++ standard library 6 | * and contains some functions copied form it it is distributed under the 7 | * same licanse as GCC. 8 | */ 9 | 10 | // Under Section 7 of GPL version 3, you are granted additional 11 | // permissions described in the GCC Runtime Library Exception, version 12 | // 3.1, as published by the Free Software Foundation. 13 | 14 | // You should have received a copy of the GNU General Public License and 15 | // a copy of the GCC Runtime Library Exception along with this program; 16 | // see the files COPYING3 and COPYING.RUNTIME respectively. If not, see 17 | // . 18 | 19 | // Do not change anything for plain C users 20 | #ifdef __cplusplus 21 | 22 | // Undef macro horror 23 | 24 | #undef min 25 | #undef max 26 | #undef abs 27 | //#undef constrain // not working for float 28 | #undef radians 29 | #undef degrees 30 | #undef sq 31 | 32 | // So as to stay as much API compatible as possible min, max and abs 33 | // are not placed in std namespace 34 | 35 | template 36 | inline auto min(const Tpa& a, const Tpb& b) -> decltype(a < b ? a : b) { 37 | return b < a ? b : a; 38 | } 39 | 40 | template 41 | inline auto max(const Tpa& a, const Tpb& b) -> decltype(b > a ? b : a) { 42 | return b > a ? b : a; 43 | } 44 | 45 | inline double abs(double x) { 46 | return __builtin_fabs(x); 47 | } 48 | 49 | inline float abs(float x) { 50 | return __builtin_fabsf(x); 51 | } 52 | 53 | inline long double abs(long double x) { 54 | return __builtin_fabsl(x); 55 | } 56 | 57 | inline long abs(long i) { 58 | return labs(i); 59 | } 60 | 61 | inline long long abs(long long x) { 62 | return x >= 0 ? x : -x; 63 | } 64 | 65 | // abs for int is defined by C library 66 | //inline int abs(int i) 67 | 68 | //template 69 | //inline auto constrain( 70 | // const Tpa& amt, 71 | // const Tpb& low, 72 | // const Tpc& high) -> decltype(min(max(amt, low), high)) { 73 | // return min(max(amt, low), high); 74 | //} 75 | 76 | inline float radians(float deg) { 77 | return deg * DEG_TO_RAD; 78 | } 79 | 80 | inline double radians(double deg) { 81 | return deg * DEG_TO_RAD; 82 | } 83 | 84 | inline float degrees(float deg) { 85 | return deg * RAD_TO_DEG; 86 | } 87 | 88 | inline double degrees(double deg) { 89 | return deg * RAD_TO_DEG; 90 | } 91 | 92 | /// This one requires C++11 sintax 93 | template 94 | inline auto sqr(const Tp& x) -> decltype(x * x) { 95 | return x * x; 96 | } 97 | 98 | // We have malloc and free. Why not to have new and delete? 99 | // Use them with caution so as not to end up with laggy application. 100 | inline void * operator new(size_t size) { return malloc(size); } 101 | inline void operator delete(void* ptr) { free(ptr); } 102 | 103 | #endif // __cplusplus 104 | 105 | #endif // CPP_UTILS_H_INCLUDED 106 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map for RoboClaw 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | RoboClaw KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | begin KEYWORD2 16 | ForwardM1 KEYWORD2 17 | BackwardM1 KEYWORD2 18 | SetMinVoltageMainBattery KEYWORD2 19 | SetMaxVoltageMainBattery KEYWORD2 20 | ForwardM2 KEYWORD2 21 | BackwardM2 KEYWORD2 22 | ForwardBackwardM1 KEYWORD2 23 | ForwardBackwardM2 KEYWORD2 24 | ForwardMixed KEYWORD2 25 | BackwardMixed KEYWORD2 26 | TurnRightMixed KEYWORD2 27 | TurnLeftMixed KEYWORD2 28 | ForwardBackwardMixed KEYWORD2 29 | LeftRightMixed KEYWORD2 30 | ReadEncM1 KEYWORD2 31 | ReadEncM2 KEYWORD2 32 | SetEncM1 KEYWORD2 33 | SetEncM2 KEYWORD2 34 | ReadSpeedM1 KEYWORD2 35 | ReadSpeedM2 KEYWORD2 36 | ResetEncoders KEYWORD2 37 | ReadVersion KEYWORD2 38 | ReadMainBatteryVoltage KEYWORD2 39 | ReadLogicBattVoltage KEYWORD2 40 | SetMinVoltageLogicBattery KEYWORD2 41 | SetMaxVoltageLogicBattery KEYWORD2 42 | SetM1VelocityPID KEYWORD2 43 | SetM2VelocityPID KEYWORD2 44 | ReadISpeedM1 KEYWORD2 45 | ReadISpeedM2 KEYWORD2 46 | DutyM1 KEYWORD2 47 | DutyM2 KEYWORD2 48 | DutyM1M2 KEYWORD2 49 | SpeedM1 KEYWORD2 50 | SpeedM2 KEYWORD2 51 | SpeedM1M2 KEYWORD2 52 | SpeedAccelM1 KEYWORD2 53 | SpeedAccelM2 KEYWORD2 54 | SpeedAccelM1M2 KEYWORD2 55 | SpeedDistanceM1 KEYWORD2 56 | SpeedDistanceM2 KEYWORD2 57 | SpeedDistanceM1M2 KEYWORD2 58 | SpeedAccelDistanceM1 KEYWORD2 59 | SpeedAccelDistanceM2 KEYWORD2 60 | SpeedAccelDistanceM1M2 KEYWORD2 61 | ReadBuffers KEYWORD2 62 | ReadPWMs KEYWORD2 63 | ReadCurrents KEYWORD2 64 | SpeedAccelM1M2_2 KEYWORD2 65 | SpeedAccelDistanceM1M2_2 KEYWORD2 66 | DutyAccelM1 KEYWORD2 67 | DutyAccelM2 KEYWORD2 68 | DutyAccelM1M2 KEYWORD2 69 | ReadM1VelocityPID KEYWORD2 70 | ReadM2VelocityPID KEYWORD2 71 | SetMainVoltages KEYWORD2 72 | SetLogicVoltages KEYWORD2 73 | ReadMinMaxMainVoltages KEYWORD2 74 | ReadMinMaxLogicVoltages KEYWORD2 75 | SetM1PositionPID KEYWORD2 76 | SetM2PositionPID KEYWORD2 77 | ReadM1PositionPID KEYWORD2 78 | ReadM2PositionPID KEYWORD2 79 | SpeedAccelDeccelPositionM1 KEYWORD2 80 | SpeedAccelDeccelPositionM2 KEYWORD2 81 | SpeedAccelDeccelPositionM1M2 KEYWORD2 82 | SetM1DefaultAccel KEYWORD2 83 | SetM2DefaultAccel KEYWORD2 84 | SetPinFunctions KEYWORD2 85 | GetPinFunctions KEYWORD2 86 | SetDeadBand KEYWORD2 87 | GetDeadBand KEYWORD2 88 | RestoreDefaults KEYWORD2 89 | ReadTemp KEYWORD2 90 | ReadTemp2 KEYWORD2 91 | ReadError KEYWORD2 92 | ReadEncoderModes KEYWORD2 93 | SetM1EncoderMode KEYWORD2 94 | SetM2EncoderMode KEYWORD2 95 | WriteNVM KEYWORD2 96 | ReadNVM KEYWORD2 97 | SetConfig KEYWORD2 98 | GetConfig KEYWORD2 99 | SetM1MaxCurrent KEYWORD2 100 | SetM2MaxCurrent KEYWORD2 101 | ReadM1MaxCurrent KEYWORD2 102 | ReadM2MaxCurrent KEYWORD2 103 | SetPWMMode KEYWORD2 104 | GetPWMMode KEYWORD2 105 | 106 | ####################################### 107 | # Constants (LITERAL1) 108 | ####################################### 109 | 110 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderSpeedAccelDistance/PacketSerialEncoderSpeedAccelDistance.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Velocity PID coefficients 14 | #define Kp 1.0 15 | #define Ki 0.5 16 | #define Kd 0.25 17 | #define qpps 44000 18 | 19 | void setup() { 20 | //Open Serial and roboclaw serial ports 21 | Serial.begin(57600); 22 | roboclaw.begin(38400); 23 | 24 | //Set PID Coefficients 25 | roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); 26 | roboclaw.SetM2VelocityPID(address,Kd,Kp,Ki,qpps); 27 | } 28 | 29 | void displayspeed(void) 30 | { 31 | uint8_t status1,status2,status3,status4; 32 | bool valid1,valid2,valid3,valid4; 33 | 34 | int32_t enc1= roboclaw.ReadEncM1(address, &status1, &valid1); 35 | int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2); 36 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3); 37 | int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4); 38 | Serial.print("Encoder1:"); 39 | if(valid1){ 40 | Serial.print(enc1,DEC); 41 | Serial.print(" "); 42 | Serial.print(status1,HEX); 43 | Serial.print(" "); 44 | } 45 | else{ 46 | Serial.print("failed "); 47 | } 48 | Serial.print("Encoder2:"); 49 | if(valid2){ 50 | Serial.print(enc2,DEC); 51 | Serial.print(" "); 52 | Serial.print(status2,HEX); 53 | Serial.print(" "); 54 | } 55 | else{ 56 | Serial.print("failed "); 57 | } 58 | Serial.print("Speed1:"); 59 | if(valid3){ 60 | Serial.print(speed1,DEC); 61 | Serial.print(" "); 62 | } 63 | else{ 64 | Serial.print("failed "); 65 | } 66 | Serial.print("Speed2:"); 67 | if(valid4){ 68 | Serial.print(speed2,DEC); 69 | Serial.print(" "); 70 | } 71 | else{ 72 | Serial.print("failed "); 73 | } 74 | Serial.println(); 75 | } 76 | 77 | void loop() { 78 | uint8_t depth1,depth2; 79 | 80 | roboclaw.SpeedAccelDistanceM1(address,12000,12000,42000,1); 81 | roboclaw.SpeedAccelDistanceM2(address,12000,-12000,42000,1); 82 | roboclaw.SpeedAccelDistanceM1(address,12000,0,0); //distance traveled is v*v/2a = 12000*12000/2*12000 = 6000 83 | roboclaw.SpeedAccelDistanceM2(address,12000,0,0); //that makes the total move in ondirection 48000 84 | do{ 85 | displayspeed(); 86 | roboclaw.ReadBuffers(address,depth1,depth2); 87 | }while(depth1!=0x80 && depth2!=0x80); //loop until distance command completes 88 | 89 | delay(1000); 90 | 91 | roboclaw.SpeedAccelDistanceM1(address,12000,-12000,42000,1); 92 | roboclaw.SpeedAccelDistanceM2(address,12000,12000,42000,1); 93 | roboclaw.SpeedAccelDistanceM1(address,12000,0,0); 94 | roboclaw.SpeedAccelDistanceM2(address,12000,0,0); 95 | do{ 96 | displayspeed(); 97 | roboclaw.ReadBuffers(address,depth1,depth2); 98 | }while(depth1!=0x80 && depth2!=0x80); //loop until distance command completes 99 | 100 | delay(1000); 101 | 102 | } 103 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![build](https://github.com/mit-drl/e-vent/workflows/build/badge.svg) 2 | 3 | # MIT E-Vent: A Low Cost Emergency Ventilator Controller 4 | 5 | ![MIT E-Vent Prototype](https://user-images.githubusercontent.com/17116105/80438039-39f32580-88d1-11ea-8a16-f9bce3a209bf.jpg) 6 | 7 | **This code is provided for reference use only.** Our goal is to help others understand the function of our prototype system, with regards to: delivering tidal volumes, breaths per minute and I:E ratios; visible and audible alarm functions; and the user interface. Assist control, to detect patient breathing, is implemented, but is still a work in progress. (Synchronizing with a human is non-trivial.) These features are only a baseline and others will likely wish to add more safeties and features. 8 | 9 | _Caution: Any group working to develop ventilation or other medical device must conduct significant hardware and software validation and testing to identify and mitigate fault conditions. This is essential to ensure patient safety._ 10 | 11 | Please see [https://e-vent.mit.edu/](https://e-vent.mit.edu/) for a high level description of the complete system. 12 | 13 | 14 | ### Volume Control Mode Definitions 15 | The following waveform diagram is a visual explanation of the key cycle parameters used in [e-vent.ino](e-vent.ino). 16 | 17 | | 18 | | |<- Inspiration ->|<- Expiration ->| 19 | P | 20 | r | | | | 21 | e | PIP -> / / 22 | s | | /| | /| 23 | s | / |__ <- Plateau / |__ 24 | u | | / \ | / \ 25 | r | / \ / \ 26 | e | | / \ | / \ 27 | | / \ / \ 28 | | | / \ | / \ 29 | | / \ / \ 30 | | | / \ | / 31 | | / \ / 32 | | | / \ | / 33 | | / \ / 34 | | / \ / 35 | | _____/ PEEP -> \______/ 36 | |___________________________________________________________________ 37 | | | | | | 38 | |<- tIn ----->| | | | Time 39 | |<- tHoldIn ---->| | | 40 | |<- tEx ------------------->| | 41 | |<- tPeriod ---------------------->| 42 | | 43 | 44 | tCycleTimer 45 | 46 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderSpeedDistance/PacketSerialEncoderSpeedDistance.ino: -------------------------------------------------------------------------------- 1 | //See BareMinimum example for a list of library functions 2 | 3 | //Includes required to use Roboclaw library 4 | #include 5 | #include "RoboClaw.h" 6 | 7 | //See limitations of Arduino SoftwareSerial 8 | SoftwareSerial serial(10,11); 9 | RoboClaw roboclaw(&serial,10000); 10 | 11 | #define address 0x80 12 | 13 | //Velocity PID coefficients 14 | #define Kp 1.0 15 | #define Ki 0.5 16 | #define Kd 0.25 17 | #define qpps 44000 18 | 19 | void setup() { 20 | //Open Serial and roboclaw serial ports 21 | Serial.begin(57600); 22 | roboclaw.begin(38400); 23 | 24 | //Set PID Coefficients 25 | roboclaw.SetM1VelocityPID(address,Kd,Kp,Ki,qpps); 26 | roboclaw.SetM2VelocityPID(address,Kd,Kp,Ki,qpps); 27 | } 28 | 29 | void displayspeed(void) 30 | { 31 | uint8_t status1,status2,status3,status4; 32 | bool valid1,valid2,valid3,valid4; 33 | 34 | int32_t enc1 = roboclaw.ReadEncM1(address, &status1, &valid1); 35 | int32_t enc2 = roboclaw.ReadEncM2(address, &status2, &valid2); 36 | int32_t speed1 = roboclaw.ReadSpeedM1(address, &status3, &valid3); 37 | int32_t speed2 = roboclaw.ReadSpeedM2(address, &status4, &valid4); 38 | 39 | Serial.print("Encoder1:"); 40 | if(valid1){ 41 | Serial.print(enc1,DEC); 42 | Serial.print(" "); 43 | Serial.print(status1,HEX); 44 | Serial.print(" "); 45 | } 46 | else{ 47 | Serial.print("failed "); 48 | } 49 | Serial.print("Encoder2:"); 50 | if(valid2){ 51 | Serial.print(enc2,DEC); 52 | Serial.print(" "); 53 | Serial.print(status2,HEX); 54 | Serial.print(" "); 55 | } 56 | else{ 57 | Serial.print("failed "); 58 | } 59 | Serial.print("Speed1:"); 60 | if(valid3){ 61 | Serial.print(speed1,DEC); 62 | Serial.print(" "); 63 | } 64 | else{ 65 | Serial.print("failed "); 66 | } 67 | Serial.print("Speed2:"); 68 | if(valid4){ 69 | Serial.print(speed2,DEC); 70 | Serial.print(" "); 71 | } 72 | else{ 73 | Serial.print("failed "); 74 | } 75 | Serial.println(); 76 | } 77 | 78 | void loop() { 79 | uint8_t depth1,depth2; 80 | 81 | roboclaw.SpeedDistanceM1(address,12000,48000,1); 82 | roboclaw.SpeedDistanceM2(address,-12000,48000,1); 83 | do{ 84 | displayspeed(); 85 | roboclaw.ReadBuffers(address,depth1,depth2); 86 | }while(depth1!=0x80 && depth2!=0x80); //Loop until distance command has completed 87 | 88 | delay(1000); 89 | 90 | roboclaw.SpeedDistanceM1(address,-12000,48000,1); 91 | roboclaw.SpeedDistanceM2(address,12000,48000,1); 92 | do{ 93 | displayspeed(); 94 | roboclaw.ReadBuffers(address,depth1,depth2); 95 | }while(depth1!=0x80 && depth2!=0x80); //Loop until distance command has completed 96 | 97 | delay(1000); //When no second command is given the motors will automatically slow down to 0 which takes 1 second 98 | 99 | roboclaw.SpeedDistanceM1(address,12000,48000,1); 100 | roboclaw.SpeedDistanceM2(address,-12000,48000,1); 101 | roboclaw.SpeedDistanceM1(address,-12000,48000,0); 102 | roboclaw.SpeedDistanceM2(address,12000,48000,0); 103 | roboclaw.SpeedDistanceM1(address,0,48000,0); 104 | roboclaw.SpeedDistanceM2(address,0,48000,0); 105 | do{ 106 | displayspeed(); 107 | roboclaw.ReadBuffers(address,depth1,depth2); 108 | }while(depth1!=0x80 && depth2!=0x80); //Loop until distance command has completed 109 | 110 | delay(1000); 111 | } 112 | -------------------------------------------------------------------------------- /Utilities.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Utilities.h 29 | * Defines a variety of functions used in `e-vent.ino` or other files. 30 | */ 31 | 32 | #ifndef Utilities_h 33 | #define Utilities_h 34 | 35 | #include "Arduino.h" 36 | #include "src/thirdparty/RoboClaw/RoboClaw.h" 37 | 38 | #include "cpp_utils.h" 39 | 40 | #include "Constants.h" 41 | 42 | 43 | namespace utils { 44 | 45 | 46 | /** 47 | * Pulse 48 | * Generates an ON/OFF signal with given period and duty. 49 | */ 50 | class Pulse { 51 | public: 52 | Pulse(const unsigned long& period, const float& duty, const bool& random_offset = true); 53 | 54 | // Read current ON/OFF value 55 | bool read(); 56 | 57 | private: 58 | const unsigned long period_; 59 | const unsigned long on_duration_; 60 | const unsigned long offset_; 61 | }; 62 | 63 | 64 | // Define map for floats 65 | float map(float x, float in_min, float in_max, float out_min, float out_max); 66 | 67 | // Converts motor position in ticks to volume in mL 68 | float ticks2volume(const float& vol_ticks); 69 | 70 | // Converts volume in mL to motor position in ticks 71 | float volume2ticks(const float& vol_ml); 72 | 73 | // Returns the current time in seconds 74 | inline float now() { return millis()*1e-3; } 75 | 76 | // Home switch 77 | inline bool homeSwitchPressed() { return digitalRead(HOME_PIN) == LOW; } 78 | 79 | /// Pots /// 80 | float readVolume(); // Reads set volume (in mL) from the volume pot 81 | float readBpm(); // Reads set bpm from the bpm pot 82 | float readIeRatio(); // Reads set IE ratio from the IE pot 83 | float readAc(); // Reads set AC mode trigger sensitivity from the AC pot 84 | 85 | /// Motor /// 86 | // Read the encoder and return whether the reading is valid 87 | bool readEncoder(const RoboClaw& roboclaw, int& motorPosition); 88 | 89 | // Go to a desired position at the given speed 90 | void goToPosition(const RoboClaw& roboclaw, const long& pos, const long& vel, const long& acc); 91 | 92 | // Go to a desired position over the specified duration 93 | void goToPositionByDur(const RoboClaw& roboclaw, const long& goal_pos, const long& cur_pos, const float& dur); 94 | 95 | // Read the motor current and return whether the reading is valid 96 | bool readMotorCurrent(const RoboClaw& roboclaw, int& motorCurrent); 97 | 98 | 99 | } // namespace utils 100 | 101 | 102 | #endif 103 | 104 | -------------------------------------------------------------------------------- /Input.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Input.cpp 29 | */ 30 | 31 | #include "Input.h" 32 | 33 | 34 | namespace input { 35 | 36 | 37 | /// Input /// 38 | 39 | template 40 | void Input::begin(float (*read_fun)()) { 41 | read_fun_ = read_fun; 42 | set_value_ = discretize(read_fun_()); 43 | } 44 | 45 | template 46 | T Input::discretize(const float& raw_value) const { 47 | return round(raw_value / resolution_) * resolution_; 48 | } 49 | 50 | template 51 | void Input::display(const T& value, const bool& blank) { 52 | // throttle display rate 53 | const unsigned long time_now = millis(); 54 | if (time_now - last_display_update_time_ < kDisplayUpdatePeriod) return; 55 | last_display_update_time_ = time_now; 56 | 57 | if (blank) { 58 | displ_->writeBlank(disp_key_); 59 | } 60 | else { 61 | displ_->write(disp_key_, value); 62 | } 63 | } 64 | 65 | 66 | /// Knob /// 67 | 68 | template 69 | void Knob::update() { 70 | this->set_value_ = discretize(read_fun_()); 71 | this->display(read()); 72 | } 73 | 74 | 75 | /// SafeKnob /// 76 | 77 | template 78 | void SafeKnob::begin(T (*read_fun)()) { 79 | Input::begin(read_fun); 80 | confirm_button_.begin(); 81 | } 82 | 83 | template 84 | void SafeKnob::update() { 85 | unconfirmed_value_ = discretize(read_fun_()); 86 | if (abs(unconfirmed_value_ - this->set_value_) <= this->resolution_ / 2) { 87 | this->display(read()); 88 | if (!confirmed_) { 89 | confirmed_ = true; 90 | alarms_->unconfirmedChange(false); 91 | } 92 | } 93 | else if (confirm_button_.is_LOW()) { 94 | this->set_value_ = unconfirmed_value_; 95 | } 96 | else { 97 | const unsigned long time_now = millis(); 98 | if (confirmed_) { 99 | time_changed_ = time_now; 100 | confirmed_ = false; 101 | } 102 | this->display(unconfirmed_value_, !pulse_.read()); 103 | if (time_now - time_changed_ > kAlarmTime) { 104 | alarms_->unconfirmedChange(true, getConfirmPrompt()); 105 | } 106 | } 107 | } 108 | 109 | template 110 | String SafeKnob::getConfirmPrompt() const { 111 | char buff[display::kWidth + 1]; 112 | sprintf(buff, "Set %s(%s)->%s?", this->getLabel().c_str(), 113 | this->toString(this->set_value_).c_str(), this->toString(unconfirmed_value_).c_str()); 114 | String s(buff); 115 | while (s.length() < display::kWidth) s += " "; 116 | return s; 117 | } 118 | 119 | 120 | } 121 | -------------------------------------------------------------------------------- /Utilities.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Utilities.cpp 29 | */ 30 | 31 | #include "Utilities.h" 32 | 33 | 34 | namespace utils { 35 | 36 | 37 | /// Pulse /// 38 | 39 | Pulse::Pulse(const unsigned long& period, const float& duty, const bool& random_offset): 40 | period_(period), 41 | on_duration_(duty * period), 42 | offset_(random_offset ? random(period) : 0) {} 43 | 44 | bool Pulse::read() { 45 | return (millis() - offset_) % period_ < on_duration_; 46 | } 47 | 48 | float map(float x, float in_min, float in_max, float out_min, float out_max) { 49 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 50 | } 51 | 52 | float ticks2volume(const float& vol_ticks) { 53 | return COEFFS.a * sqr(vol_ticks) + COEFFS.b * vol_ticks + COEFFS.c; 54 | } 55 | 56 | float volume2ticks(const float& vol_ml) { 57 | return (-COEFFS.b + sqrt(sqr(COEFFS.b) - 4 * COEFFS.a * (COEFFS.c - vol_ml))) / (2 * COEFFS.a); 58 | } 59 | 60 | float readVolume() { 61 | return map(analogRead(VOL_PIN), 0, ANALOG_PIN_MAX, VOL_MIN, VOL_MAX); 62 | } 63 | 64 | float readBpm() { 65 | return map(analogRead(BPM_PIN), 0, ANALOG_PIN_MAX, BPM_MIN, BPM_MAX); 66 | } 67 | 68 | float readIeRatio() { 69 | return map(analogRead(IE_PIN), 0, ANALOG_PIN_MAX, IE_MIN, IE_MAX); 70 | } 71 | 72 | float readAc() { 73 | return map(analogRead(AC_PIN), 0, ANALOG_PIN_MAX, AC_MIN - AC_RES, AC_MAX); 74 | } 75 | 76 | bool readEncoder(const RoboClaw& roboclaw, int& motorPosition) { 77 | uint8_t robot_status; 78 | bool valid; 79 | motorPosition = roboclaw.ReadEncM1(ROBOCLAW_ADDR, &robot_status, &valid); 80 | return valid; 81 | } 82 | 83 | void goToPosition(const RoboClaw& roboclaw, const long& pos, const long& vel, const long& acc) { 84 | roboclaw.SpeedAccelDeccelPositionM1(ROBOCLAW_ADDR, acc, vel, acc, pos, 1); 85 | } 86 | 87 | void goToPositionByDur(const RoboClaw& roboclaw, const long& goal_pos, const long& cur_pos, const float& dur) { 88 | if (dur <= 0) return; // Can't move in negative time 89 | 90 | const long dist = abs(goal_pos - cur_pos); 91 | long vel = round(2*dist/dur); // Try bang-bang control 92 | long acc = round(2*vel/dur); // Constant acc in and out 93 | if (vel > VEL_MAX) { 94 | // Must use trapezoidal velocity profile to clip at VEL_MAX 95 | vel = VEL_MAX; 96 | const float acc_dur = dur - dist/vel; 97 | acc = acc_dur > 0 ? round(vel/acc_dur) : ACC_MAX; 98 | acc = min(ACC_MAX, acc); 99 | } 100 | 101 | goToPosition(roboclaw, goal_pos, vel, acc); 102 | } 103 | 104 | bool readMotorCurrent(const RoboClaw& roboclaw, int& motorCurrent) { 105 | int noSecondMotor; 106 | const bool valid = roboclaw.ReadCurrents(ROBOCLAW_ADDR, motorCurrent, noSecondMotor); 107 | return valid; 108 | } 109 | 110 | 111 | } // namespace utils 112 | -------------------------------------------------------------------------------- /Input.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Input.h 29 | * Defines interface for knobs used in the E-Vent. There are two implementations: 30 | * - `Knob` the simplest of the two with a `read()` method that just returns the value 31 | * currently dialed. 32 | * - `SafeKnob`, with a `read()` method that returns the last value confirmed but also 33 | * sets off an alarm if a changed value is not confirmed. 34 | */ 35 | 36 | #ifndef Input_h 37 | #define Input_h 38 | 39 | #include "Arduino.h" 40 | 41 | #include "Alarms.h" 42 | #include "Buttons.h" 43 | #include "Display.h" 44 | #include "Utilities.h" 45 | 46 | 47 | namespace input { 48 | 49 | 50 | using alarms::AlarmManager; 51 | using display::Display; 52 | 53 | 54 | /** 55 | * Input 56 | * Defines the interface for different possible input methods, e.g. knobs. 57 | */ 58 | template 59 | class Input { 60 | static const unsigned long kDisplayUpdatePeriod = 250; 61 | 62 | public: 63 | Input(Display* displ, const display::DisplayKey& key, const T& resolution): 64 | displ_(displ), 65 | disp_key_(key), 66 | resolution_(resolution) {} 67 | 68 | // Setup during arduino setup() 69 | void begin(float (*read_fun)()); 70 | 71 | // Update during arduino loop() 72 | virtual void update() = 0; 73 | 74 | // Read confirmed value to use for operation 75 | inline T read() const& { return set_value_; } 76 | 77 | protected: 78 | float (*read_fun_)(); 79 | Display* displ_; 80 | display::DisplayKey disp_key_; 81 | T resolution_; 82 | 83 | T set_value_; // Dial value displayed and used for operation 84 | unsigned long last_display_update_time_ = 0; 85 | 86 | // Discretize value into closest multiple of resolution 87 | T discretize(const float& raw_value) const; 88 | 89 | void display(const T& value, const bool& blank = false); 90 | 91 | inline String toString(const T& val) const { return displ_->toString(disp_key_, val); } 92 | 93 | inline String getLabel() const { return displ_->getLabel(disp_key_); } 94 | }; 95 | 96 | 97 | /** 98 | * Knob 99 | * Simplest knob interface that directly sets value set from pot. 100 | */ 101 | template 102 | class Knob : public Input { 103 | public: 104 | Knob(Display* displ, const display::DisplayKey& key, const T& resolution): 105 | Input(displ, key, resolution) {} 106 | 107 | // Update during arduino loop() 108 | void update(); 109 | }; 110 | 111 | // Instantiate for types used 112 | template class Knob; 113 | template class Knob; 114 | 115 | 116 | /** 117 | * SafeKnob 118 | * Safe knob that requires confirmation via 'confirm' button before setting a value. 119 | */ 120 | template 121 | class SafeKnob : public Input { 122 | 123 | // Time to wait to sound alarm after knob is changed if not confirmed 124 | static const unsigned long kAlarmTime = 5 * 1000UL; 125 | 126 | public: 127 | SafeKnob(Display* displ, const display::DisplayKey& key, 128 | const int& confirm_pin, AlarmManager* alarms, const T& resolution): 129 | Input(displ, key, resolution), 130 | confirm_button_(confirm_pin), 131 | alarms_(alarms), 132 | pulse_(1000, 0.5) {} 133 | 134 | // Setup during arduino setup() 135 | void begin(T (*read_fun)()); 136 | 137 | // Update during arduino loop() 138 | void update(); 139 | 140 | private: 141 | buttons::DebouncedButton confirm_button_; 142 | AlarmManager* alarms_; 143 | utils::Pulse pulse_; 144 | T unconfirmed_value_; 145 | unsigned long time_changed_ = 0; 146 | bool confirmed_ = true; 147 | 148 | String getConfirmPrompt() const; 149 | }; 150 | 151 | // Instantiate for types used 152 | template class SafeKnob; 153 | template class SafeKnob; 154 | 155 | 156 | } 157 | 158 | 159 | #endif 160 | 161 | -------------------------------------------------------------------------------- /Logging.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Logging.h 29 | * Provides common interface for logging an arbitrary number of variables 30 | * to an SD card and/or to serial, e.g. the Serial monitor. 31 | */ 32 | 33 | #ifndef Logging_h 34 | #define Logging_h 35 | 36 | #include "Arduino.h" 37 | #include 38 | #include 39 | 40 | 41 | namespace logging { 42 | 43 | /** 44 | * Var 45 | * A variable to be logged, with label and serialization methods. 46 | */ 47 | class Var { 48 | public: 49 | Var() = default; 50 | 51 | // Set var label and pointer, and min digits and decimal digits for serialization 52 | template 53 | Var(const String& label, T* var, const int& min_digits, const int& float_precision); 54 | 55 | // Get the variable label 56 | inline String label() const { return label_; } 57 | 58 | // Get a string representation of the variable pointed to 59 | String serialize() const; 60 | 61 | private: 62 | String label_; 63 | int min_digits_; 64 | int float_precision_; 65 | 66 | union { 67 | bool* b; 68 | int* i; 69 | float* f; 70 | double* d; 71 | } var_; 72 | 73 | enum Type { 74 | BOOL, 75 | INT, 76 | FLOAT, 77 | DOUBLE 78 | } type_; 79 | 80 | String pad(String& s); 81 | 82 | void setPtr(const bool* var); 83 | 84 | void setPtr(const int* var); 85 | 86 | void setPtr(const float* var); 87 | 88 | void setPtr(const double* var); 89 | 90 | String serialize(bool* var) const; 91 | 92 | String serialize(int* var) const; 93 | 94 | String serialize(float* var) const; 95 | 96 | String serialize(double* var) const; 97 | }; 98 | 99 | 100 | /** 101 | * Logger 102 | * Handles logging to serial or SD card. 103 | * 104 | * Example usage: 105 | * 106 | * const bool log_to_serial = true; 107 | * const bool log_to_card = true; 108 | * 109 | * logging::Logger logger(log_to_serial, log_to_card); 110 | * 111 | * setup() { 112 | * logger.addVar("var1_label", &var1); 113 | * logger.addVar("var2_label", &var2); 114 | * ... 115 | * logger.addVar("varN_label", &varN); 116 | * 117 | * const int sd_select_pin = 53; 118 | * logger.begin(&Serial, sd_select_pin); 119 | * } 120 | * 121 | * loop() { 122 | * var1 = ... 123 | * var2 = ... 124 | * ... 125 | * varN = ... 126 | * 127 | * logger.update(); 128 | * } 129 | */ 130 | class Logger { 131 | 132 | // Period for saving the file 133 | const unsigned long kSavePeriod = 1 * 1000UL; 134 | 135 | // Maximum number of variables to log 136 | static const int kMaxVars = 20; 137 | 138 | public: 139 | // Set options 140 | Logger(bool log_to_serial, bool log_to_SD, 141 | bool serial_labels = true, const String delim = "\t"); 142 | 143 | // Add variable 144 | template 145 | void addVar(const char var_name[], const T* var, 146 | const int& min_digits = 1, const int& float_precision = 2); 147 | 148 | // Setup during arduino setup() 149 | // call after adding all the vars for the header to have them all 150 | void begin(const Stream* serial, const int& pin_select_SD); 151 | 152 | // Update during arduino loop() 153 | // Write all the variables to stream object and/or SD card 154 | void update(); 155 | 156 | private: 157 | // Options 158 | const bool log_to_serial_, log_to_SD_, serial_labels_; 159 | const String delim_; 160 | 161 | // Stream objects 162 | Stream* stream_; 163 | char filename_[12] = "DATA000.TXT"; 164 | File file_; 165 | 166 | // Bookkeeping 167 | unsigned long last_save_ = 0; 168 | Var vars_[kMaxVars]; 169 | int num_vars_ = 0; 170 | 171 | void makeFile(); 172 | }; 173 | 174 | // Instantiation of template methods 175 | #define INSTANTIATE_ADDVAR(vartype) \ 176 | template void Logger::addVar(const char var_name[], const vartype* var, \ 177 | const int& min_digits, const int& float_precision); 178 | INSTANTIATE_ADDVAR(bool) 179 | INSTANTIATE_ADDVAR(int) 180 | INSTANTIATE_ADDVAR(float) 181 | INSTANTIATE_ADDVAR(double) 182 | #undef INSTANTIATE 183 | 184 | 185 | } // namespace logging 186 | 187 | #endif 188 | -------------------------------------------------------------------------------- /Constants.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Constants.h 29 | * Defines all the global constants used in `e-vent.ino`. 30 | */ 31 | 32 | #ifndef Constants_h 33 | #define Constants_h 34 | 35 | // States 36 | enum States { 37 | DEBUG_STATE, // 0 38 | IN_STATE, // 1 39 | HOLD_IN_STATE, // 2 40 | EX_STATE, // 3 41 | PEEP_PAUSE_STATE, // 4 42 | HOLD_EX_STATE, // 5 43 | PREHOME_STATE, // 6 44 | HOMING_STATE, // 7 45 | OFF_STATE // 8 46 | }; 47 | 48 | // Serial baud rate 49 | const long SERIAL_BAUD_RATE = 115200; 50 | 51 | // Flags 52 | const bool DEBUG = false; // For controlling and displaying via serial 53 | const bool ASSIST_CONTROL = false; // Enable assist control 54 | 55 | // Timing Settings 56 | const float LOOP_PERIOD = 0.03; // The period (s) of the control loop 57 | const float HOLD_IN_DURATION = 0.1; // Duration (s) to pause after inhalation 58 | const float MIN_PEEP_PAUSE = 0.05; // Time (s) to pause after exhalation / before watching for an assisted inhalation 59 | const float MAX_EX_DURATION = 1.00; // Maximum exhale duration (s) 60 | 61 | // Homing Settings 62 | const float HOMING_VOLTS = 30; // The speed (0-255) in volts to use during homing 63 | const float HOMING_PAUSE = 1.0; // The pause time (s) during homing to ensure stability 64 | const int BAG_CLEAR_POS = 50; // The goal position (clicks) to retract to clear the bag 65 | const int BAG_CLEAR_TOL = 10; // The tolerance (clicks) to consider clear of bag 66 | 67 | // Pins 68 | const int VOL_PIN = A0; 69 | const int BPM_PIN = A1; 70 | const int IE_PIN = A2; 71 | const int AC_PIN = A3; 72 | const int PRESS_SENSE_PIN = A4; 73 | const int HOME_PIN = 10; 74 | const int BEEPER_PIN = 11; 75 | const int SNOOZE_PIN = 43; 76 | const int CONFIRM_PIN = 41; 77 | const int SD_SELECT = 53; 78 | const int OFF_PIN = 45; 79 | const int LED_ALARM_PIN = 12; 80 | const int LCD_RS_PIN = 9; 81 | const int LCD_EN_PIN = 8; 82 | const int LCD_D4_PIN = 7; 83 | const int dLCD_D5_PIN = 6; 84 | const int LCD_D6_PIN = 5; 85 | const int LCD_D7_PIN = 4; 86 | 87 | // Control knob mappings 88 | const int BPM_MIN = 10; 89 | const int BPM_MAX = 35; 90 | const int BPM_RES = 1; 91 | const float IE_MIN = 1; 92 | const float IE_MAX = 4; 93 | const float IE_RES = 0.1; 94 | const int VOL_MIN = 100; 95 | const int VOL_MAX = 800; 96 | const int VOL_RES = 25; 97 | const float AC_MIN = 2; 98 | const float AC_MAX = 5; 99 | const float AC_RES = 0.1; 100 | const int ANALOG_PIN_MAX = 1023; // The maximum count on analog pins 101 | 102 | // Bag Calibration for AMBU Adult bag 103 | const struct {float a, b, c;} COEFFS{1.29083271e-03, 4.72985182e-01, -7.35403067e+01}; 104 | 105 | // Safety settings 106 | const float MAX_PRESSURE = 40.0; // Trigger high pressure alarm 107 | const float MIN_PLATEAU_PRESSURE = 5.0; // Trigger low pressure alarm 108 | const float MAX_RESIST_PRESSURE = 2.0; // Trigger high-resistance notification 109 | const float MIN_TIDAL_PRESSURE = 5.0; // Trigger no-tidal-pressure alarm 110 | const float VOLUME_ERROR_THRESH = 50.0; // Trigger incomplete breath alarm 111 | const int MAX_MOTOR_CURRENT = 1000; // Trigger mechanical failure alarm. Units (10mA) 112 | const float TURNING_OFF_DURATION = 5.0; // Turning-off alarm is on for this duration (s) 113 | const float MECHANICAL_TIMEOUT = 1.0; // Time to wait for the mechanical cycle to finish before alarming 114 | 115 | // PID values for auto-tuned for PG188 116 | const unsigned long QPPS = 2000; 117 | const float VKP = 6.38650; 118 | const float VKI = 0.0; 119 | const float VKD = 0.0; 120 | const float PKP = 70.0; 121 | const float PKI = 0.2; 122 | const float PKD = 200.0; 123 | const unsigned long KI_MAX = 10; 124 | const unsigned long DEADZONE = 0; 125 | const unsigned long MIN_POS = -100; 126 | const unsigned long MAX_POS = 700; 127 | const unsigned long VEL_MAX = 1800; // Maximum velocity (clicks/s) to command 128 | const unsigned long ACC_MAX = 200000; // Maximum acceleration (clicks/s^2) to command 129 | 130 | // Roboclaw 131 | const unsigned int ROBOCLAW_ADDR = 0x80; 132 | const long ROBOCLAW_BAUD = 38400; 133 | const unsigned long ROBOCLAW_MAX_CURRENT = 2000; //Safety shutoff in units of 10mA 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /Logging.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Logging.cpp 29 | */ 30 | 31 | #include "Logging.h" 32 | 33 | 34 | namespace logging { 35 | 36 | 37 | /// Var 38 | 39 | template 40 | Var::Var(const String& label, T* var, const int& min_digits, const int& float_precision): 41 | label_(label), 42 | min_digits_(min_digits), 43 | float_precision_(float_precision) { 44 | setPtr(var); 45 | } 46 | 47 | 48 | String Var::serialize() const { 49 | switch (type_) { 50 | case BOOL: 51 | return serialize(var_.b); 52 | case INT: 53 | return serialize(var_.i); 54 | case FLOAT: 55 | return serialize(var_.f); 56 | case DOUBLE: 57 | return serialize(var_.d); 58 | } 59 | } 60 | 61 | String Var::pad(String& s) { 62 | while (s.length() < min_digits_) { 63 | s = " " + s; 64 | } 65 | return s; 66 | } 67 | 68 | void Var::setPtr(const bool* var) { 69 | var_.b = var; 70 | type_ = BOOL; 71 | } 72 | 73 | void Var::setPtr(const int* var) { 74 | var_.i = var; 75 | type_ = INT; 76 | } 77 | 78 | void Var::setPtr(const float* var) { 79 | var_.f = var; 80 | type_ = FLOAT; 81 | } 82 | 83 | void Var::setPtr(const double* var) { 84 | var_.d = var; 85 | type_ = DOUBLE; 86 | } 87 | 88 | String Var::serialize(bool* var) const { 89 | String string_out((int)*var); 90 | return pad(string_out); 91 | } 92 | 93 | String Var::serialize(int* var) const { 94 | String string_out(*var); 95 | return pad(string_out); 96 | } 97 | 98 | String Var::serialize(float* var) const { 99 | String string_out(*var, float_precision_); 100 | return pad(string_out); 101 | } 102 | 103 | String Var::serialize(double* var) const { 104 | String string_out(*var, float_precision_); 105 | return pad(string_out); 106 | } 107 | 108 | 109 | /// Logger 110 | 111 | Logger::Logger(bool log_to_serial, bool log_to_SD, 112 | bool serial_labels, const String delim): 113 | log_to_serial_(log_to_serial), 114 | log_to_SD_(log_to_SD), 115 | serial_labels_(serial_labels), 116 | delim_(delim) {} 117 | 118 | template 119 | void Logger::addVar(const char var_name[], const T* var, 120 | const int& min_digits, const int& float_precision) { 121 | vars_[num_vars_++] = Var(var_name, var, min_digits, float_precision); 122 | } 123 | 124 | void Logger::begin(const Stream* serial, const int& pin_select_SD) { 125 | stream_ = serial; 126 | 127 | if (log_to_SD_) { 128 | pinMode(pin_select_SD, OUTPUT); 129 | 130 | if (!SD.begin(pin_select_SD)) { 131 | return; 132 | } 133 | 134 | makeFile(); 135 | } 136 | } 137 | 138 | void Logger::update() { 139 | if ((!log_to_serial_ && !log_to_SD_) || num_vars_ == 0) { 140 | return; 141 | } 142 | unsigned long time_now = millis(); 143 | String line, line_with_labels; 144 | 145 | const bool need_line_without_labels = log_to_SD_ || (log_to_serial_ && !serial_labels_); 146 | const bool need_line_with_labels = log_to_serial_ && serial_labels_; 147 | 148 | for (int i = 0; i < num_vars_; i++) { 149 | String word = vars_[i].serialize(); 150 | if (i != num_vars_ - 1) { 151 | word += delim_; 152 | } 153 | if (need_line_without_labels) { 154 | line += word; 155 | } 156 | if (need_line_with_labels) { 157 | line_with_labels += vars_[i].label() + ": " + word; 158 | } 159 | } 160 | 161 | if (log_to_serial_) { 162 | stream_->println(serial_labels_ ? line_with_labels : line); 163 | } 164 | 165 | if (log_to_SD_) { 166 | if (!file_) { 167 | file_ = SD.open(filename_, FILE_WRITE); 168 | } 169 | if (file_) { 170 | file_.println(line); 171 | } 172 | if (time_now - last_save_ > kSavePeriod) { 173 | file_.close(); 174 | last_save_ = time_now; 175 | } 176 | } 177 | } 178 | 179 | void Logger::makeFile() { 180 | // Open file with number of last saved file 181 | int num; 182 | File number_file = SD.open("number.txt", FILE_READ); 183 | if (number_file) { 184 | num = number_file.parseInt(); 185 | number_file.close(); 186 | } 187 | 188 | // Replace old number with new number 189 | SD.remove("number.txt"); 190 | number_file = SD.open("number.txt", FILE_WRITE); 191 | if (number_file) { 192 | number_file.println(num + 1); 193 | number_file.close(); 194 | } 195 | 196 | // Assign the number to the new file name 197 | snprintf(filename_, sizeof(filename_), "DATA%03d.TXT", num); 198 | 199 | // Print the header 200 | file_ = SD.open(filename_, FILE_WRITE); 201 | if (file_) { 202 | String line; 203 | for (int i = 0; i < num_vars_; i++) { 204 | line += vars_[i].label(); 205 | if (i != num_vars_ - 1) { 206 | line += delim_; 207 | } 208 | } 209 | file_.println(line); 210 | file_.close(); 211 | last_save_ = millis(); 212 | } 213 | } 214 | 215 | 216 | } // namespace logging 217 | 218 | -------------------------------------------------------------------------------- /Alarms.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Alarms.cpp 29 | */ 30 | 31 | #include "Alarms.h" 32 | 33 | 34 | namespace alarms { 35 | 36 | 37 | /// Tone /// 38 | 39 | Tone::Tone(const Note notes[], const int& notes_length, const int* pin): 40 | notes_(notes), 41 | pin_(pin), 42 | length_(notes_length), 43 | tone_step_(length_) {} 44 | 45 | void Tone::play() { 46 | if (length_ == 0) { 47 | return; 48 | } 49 | if (!playing_) { // Do once when tone starts 50 | tone_timer_ = millis(); 51 | tone_step_ = 0; 52 | playing_ = true; 53 | } 54 | tone_step_ %= length_; // Start again if tone finished 55 | if (millis() > tone_timer_) { 56 | tone(*pin_, notes_[tone_step_].note, notes_[tone_step_].duration); 57 | tone_timer_ += notes_[tone_step_].duration + notes_[tone_step_].pause; 58 | tone_step_ ++; 59 | } 60 | } 61 | 62 | 63 | /// Beeper /// 64 | 65 | void Beeper::begin() { 66 | snooze_button_.begin(); 67 | pinMode(beeper_pin_, OUTPUT); 68 | } 69 | 70 | void Beeper::update(const AlarmLevel& alarm_level) { 71 | if (snoozeButtonPressed()) { 72 | toggleSnooze(); 73 | } 74 | // check if snooze time is up 75 | if (snoozed_ && millis() - snooze_time_ > kSnoozeTime) { 76 | snoozed_ = false; 77 | } 78 | if (snoozed_) { 79 | stop(); 80 | } else { 81 | play(alarm_level); 82 | } 83 | } 84 | 85 | bool Beeper::snoozeButtonPressed() const { 86 | return snooze_button_.is_LOW(); 87 | } 88 | 89 | void Beeper::toggleSnooze() { 90 | if (snoozed_) { 91 | snoozed_ = false; 92 | } else { 93 | snoozed_ = true; 94 | snooze_time_ = millis(); 95 | } 96 | } 97 | 98 | void Beeper::play(const AlarmLevel& alarm_level) { 99 | for (int i = 0; i < NUM_LEVELS; i++) { 100 | if (i != alarm_level) { 101 | tones_[i].stop(); 102 | } 103 | } 104 | tones_[alarm_level].play(); 105 | } 106 | 107 | void Beeper::stop() { 108 | for (int i = 0; i < NUM_LEVELS; i++) { 109 | tones_[i].stop(); 110 | } 111 | } 112 | 113 | 114 | /// Alarm /// 115 | 116 | Alarm::Alarm(const String& default_text, const int& min_bad_to_trigger, 117 | const int& min_good_to_clear, const AlarmLevel& alarm_level): 118 | text_(default_text), 119 | min_bad_to_trigger_(min_bad_to_trigger), 120 | min_good_to_clear_(min_good_to_clear), 121 | alarm_level_(alarm_level) {} 122 | 123 | void Alarm::reset() { 124 | *this = Alarm::Alarm(text_, min_bad_to_trigger_, min_good_to_clear_, alarm_level_); 125 | } 126 | 127 | void Alarm::setCondition(const bool& bad, const unsigned long& seq) { 128 | if (bad) { 129 | consecutive_bad_ += (seq != last_bad_seq_); 130 | last_bad_seq_ = seq; 131 | if (!on_) { 132 | on_ = consecutive_bad_ >= min_bad_to_trigger_; 133 | } 134 | consecutive_good_ = 0; 135 | } else { 136 | consecutive_good_ += (seq != last_good_seq_); 137 | last_good_seq_ = seq; 138 | if (on_) { 139 | on_ = consecutive_good_ < min_good_to_clear_; 140 | } 141 | consecutive_bad_ = 0; 142 | } 143 | } 144 | 145 | void Alarm::setText(const String& text) { 146 | if (text.length() == display::kWidth) { 147 | text_ = text; 148 | } 149 | else if (text.length() > display::kWidth) { 150 | text_ = text.substring(0, display::kWidth); 151 | } 152 | else { 153 | text_ = text; 154 | while (text_.length() < display::kWidth) { 155 | text_ += " "; 156 | } 157 | } 158 | } 159 | 160 | 161 | /// AlarmManager /// 162 | 163 | void AlarmManager::begin() { 164 | beeper_.begin(); 165 | pinMode(led_pin_, OUTPUT); 166 | } 167 | 168 | void AlarmManager::update() { 169 | displ_->setAlarmText(getText()); 170 | AlarmLevel highest_level = getHighestLevel(); 171 | beeper_.update(highest_level); 172 | if (highest_level > NO_ALARM) { 173 | digitalWrite(led_pin_, led_pulse_.read() ? HIGH : LOW); 174 | } 175 | else { 176 | digitalWrite(led_pin_, LOW); 177 | } 178 | } 179 | 180 | void AlarmManager::allOff() { 181 | for (int i = 0; i < NUM_ALARMS; i++) { 182 | alarms_[i].reset(); 183 | } 184 | } 185 | 186 | int AlarmManager::numON() const { 187 | int num = 0; 188 | for (int i = 0; i < NUM_ALARMS; i++) { 189 | num += (int)alarms_[i].isON(); 190 | } 191 | return num; 192 | } 193 | 194 | String AlarmManager::getText() const { 195 | const int num_on = numON(); 196 | String text = ""; 197 | if (num_on > 0) { 198 | // determine which of the on alarms to display 199 | const int index = millis() % (num_on * kDisplayTime) / kDisplayTime; 200 | int count_on = 0; 201 | int i; 202 | for (i = 0; i < NUM_ALARMS; i++) { 203 | if (alarms_[i].isON()) { 204 | if (count_on++ == index) break; 205 | } 206 | } 207 | text = alarms_[i].text(); 208 | } 209 | return text; 210 | } 211 | 212 | AlarmLevel AlarmManager::getHighestLevel() const { 213 | AlarmLevel alarm_level = NO_ALARM; 214 | for (int i = 0; i < NUM_ALARMS; i++) { 215 | if (alarms_[i].isON()) { 216 | alarm_level = max(alarm_level, alarms_[i].alarmLevel()); 217 | } 218 | } 219 | return alarm_level; 220 | } 221 | 222 | 223 | } // namespace alarms 224 | -------------------------------------------------------------------------------- /Display.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Display.cpp 29 | */ 30 | 31 | #include "Display.h" 32 | 33 | namespace display { 34 | 35 | 36 | /// Display /// 37 | 38 | void Display::begin() { 39 | lcd_->begin(kWidth, kHeight); 40 | lcd_->noCursor(); 41 | update(); 42 | } 43 | 44 | void Display::update() { 45 | writeHeader(); 46 | } 47 | 48 | void Display::setAlarmText(const String& alarm) { 49 | if (animation_.text() != alarm) { 50 | animation_.reset(alarm); 51 | } 52 | } 53 | 54 | template 55 | void Display::write(const DisplayKey& key, const T& value) { 56 | // Do not write on top of alarms 57 | if (alarmsON() && elements_[key].row == 0 && key != HEADER) { 58 | return; 59 | } 60 | switch (key) { 61 | case HEADER: 62 | writeHeader(); 63 | break; 64 | case VOLUME: 65 | writeVolume(value); 66 | break; 67 | case BPM: 68 | writeBPM(value); 69 | break; 70 | case IE_RATIO: 71 | writeIEratio(value); 72 | break; 73 | case AC_TRIGGER: 74 | writeACTrigger(value); 75 | break; 76 | case PRES_LABEL: 77 | writePresLabel(); 78 | break; 79 | case PEAK_PRES: 80 | writePeakP(value); 81 | break; 82 | case PLATEAU_PRES: 83 | writePlateauP(value); 84 | break; 85 | case PEEP_PRES: 86 | writePEEP(value); 87 | break; 88 | } 89 | } 90 | 91 | void Display::writeBlank(const DisplayKey& key) { 92 | // Do not write on top of alarms 93 | if (alarmsON() && elements_[key].row == 0 && key != HEADER) { 94 | return; 95 | } 96 | write(elements_[key].row, elements_[key].col, elements_[key].blank); 97 | } 98 | 99 | void Display::writeHeader() { 100 | if (!alarmsON()) { 101 | writePresLabel(); 102 | } 103 | else { 104 | const String line = animation_.getLine(); 105 | if (line.length() > 0) { 106 | write(elements_[HEADER].row, elements_[HEADER].col, animation_.getLine()); 107 | } 108 | else { 109 | writeBlank(HEADER); 110 | } 111 | } 112 | } 113 | 114 | void Display::writeVolume(const int& vol) { 115 | const int vol_c = constrain(vol, 0, 999); 116 | char buff[12]; 117 | sprintf(buff, "%2s=%3s ", getLabel(VOLUME).c_str(), toString(VOLUME, vol_c).c_str()); 118 | write(elements_[VOLUME].row, elements_[VOLUME].col, buff); 119 | } 120 | 121 | void Display::writeBPM(const int& bpm) { 122 | const int bpm_c = constrain(bpm, 0, 99); 123 | char buff[12]; 124 | sprintf(buff, "%2s=%2s ", getLabel(BPM).c_str(), toString(BPM, bpm_c).c_str()); 125 | write(elements_[BPM].row, elements_[BPM].col, buff); 126 | } 127 | 128 | void Display::writeIEratio(const float& ie) { 129 | const float ie_c = constrain(ie, 0.0, 9.9); 130 | char buff[12]; 131 | sprintf(buff, "%2s=1:%3s ", getLabel(IE_RATIO).c_str(), toString(IE_RATIO, ie_c).c_str()); 132 | write(elements_[IE_RATIO].row, elements_[IE_RATIO].col, buff); 133 | } 134 | 135 | void Display::writeACTrigger(const float& ac_trigger) { 136 | if (animation_.empty()) { 137 | const float ac_trigger_c = constrain(ac_trigger, 0.0, 9.9); 138 | char buff[12]; 139 | const String trigger_str = toString(AC_TRIGGER, ac_trigger_c); 140 | sprintf(buff, "%2s=%3s ", getLabel(AC_TRIGGER).c_str(), trigger_str.c_str()); 141 | write(elements_[AC_TRIGGER].row, elements_[AC_TRIGGER].col, buff); 142 | } 143 | 144 | } 145 | 146 | void Display::writePresLabel() { 147 | write(elements_[PRES_LABEL].row, elements_[PRES_LABEL].col, "Pressure:"); 148 | } 149 | 150 | void Display::writePeakP(const int& peak) { 151 | const int peak_c = constrain(peak, -9, 99); 152 | char buff[10]; 153 | sprintf(buff, " %4s=%2s", getLabel(PEAK_PRES).c_str(), toString(PEAK_PRES, peak_c).c_str()); 154 | write(elements_[PEAK_PRES].row, elements_[PEAK_PRES].col, buff); 155 | } 156 | 157 | void Display::writePlateauP(const int& plat) { 158 | const int plat_c = constrain(plat, -9, 99); 159 | char buff[10]; 160 | sprintf(buff, " %4s=%2s", getLabel(PLATEAU_PRES).c_str(), 161 | toString(PLATEAU_PRES, plat_c).c_str()); 162 | write(elements_[PLATEAU_PRES].row, elements_[PLATEAU_PRES].col, buff); 163 | } 164 | 165 | void Display::writePEEP(const int& peep) { 166 | const int peep_c = constrain(peep, -9, 99); 167 | char buff[10]; 168 | sprintf(buff, " %4s=%2s", getLabel(PEEP_PRES).c_str(), toString(PEEP_PRES, peep_c).c_str()); 169 | write(elements_[PEEP_PRES].row, elements_[PEEP_PRES].col, buff); 170 | } 171 | 172 | template 173 | String Display::toString(const DisplayKey& key, const T& value) const { 174 | switch (key) { 175 | case VOLUME: 176 | return String(value); 177 | case BPM: 178 | return String(value); 179 | case IE_RATIO: 180 | return String(value, 1); 181 | case AC_TRIGGER: 182 | return (value > trigger_threshold_ - 1e-2) ? String(value, 1) : "OFF"; 183 | case PEAK_PRES: 184 | return String(value); 185 | case PLATEAU_PRES: 186 | return String(value); 187 | case PEEP_PRES: 188 | return String(value); 189 | default: 190 | // Not meant to be used for other keys 191 | return "N/A"; 192 | } 193 | } 194 | 195 | template 196 | void Display::write(const int& row, const int& col, const T& printable) { 197 | lcd_->setCursor(col, row); 198 | lcd_->print(printable); 199 | } 200 | 201 | 202 | } // namespace display 203 | -------------------------------------------------------------------------------- /Display.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Display.h 29 | * Handles writing of E-Vent-specific content to the LCD. 30 | */ 31 | 32 | #ifndef Display_h 33 | #define Display_h 34 | 35 | #include "Arduino.h" 36 | #include 37 | 38 | #include "Utilities.h" 39 | 40 | 41 | namespace display { 42 | 43 | static const int kWidth = 20; // Width of the display 44 | static const int kHeight = 4; // Height of the display 45 | 46 | 47 | /** 48 | * TextAnimation 49 | * Handles the blinking of text in the display. 50 | */ 51 | class TextAnimation { 52 | public: 53 | TextAnimation(const unsigned long& period, const float& on_fraction): 54 | pulse_(period, on_fraction) {} 55 | 56 | // Reset the text of the animation 57 | inline void reset(const String& text = "") { text_ = text; } 58 | 59 | // Check if the animation text is empty 60 | inline bool empty() { return text_.length() == 0; } 61 | 62 | // Get the animation text (original text passed to reset) 63 | inline const String& text() { return text_; } 64 | 65 | // Get the current string to display (empty string for blank) 66 | inline const String getLine() { return pulse_.read() ? text_ : ""; } 67 | 68 | private: 69 | String text_; 70 | utils::Pulse pulse_; 71 | unsigned long reset_time_; 72 | }; 73 | 74 | 75 | enum DisplayKey { 76 | HEADER, 77 | VOLUME, 78 | BPM, 79 | IE_RATIO, 80 | AC_TRIGGER, 81 | PRES_LABEL, 82 | PEAK_PRES, 83 | PLATEAU_PRES, 84 | PEEP_PRES, 85 | NUM_KEYS 86 | }; 87 | 88 | 89 | /** 90 | * Display 91 | * Handles writing ventilator-specific elements to the display. 92 | * Default format (without alarms) is as follows: 93 | * 94 | * 1111111111 95 | * 01234567890123456789 96 | * ____________________ 97 | * 0 |AC=#.# Pressure:| 98 | * 1 |TV=### peak=##| 99 | * 2 |RR=## plat=##| 100 | * 3 |IE=1:#.# PEEP=##| 101 | * ____________________ 102 | */ 103 | class Display { 104 | 105 | struct Element { 106 | Element() = default; 107 | 108 | Element(const int& r, const int& c, const int& w, const String& l = ""): 109 | row(r), col(c), width(w), label(l) { 110 | while (blank.length() < width) blank += " "; 111 | } 112 | int row; 113 | int col; 114 | int width; 115 | String label; 116 | String blank; 117 | }; 118 | 119 | public: 120 | // Constructor, save a pointer to the (global) display object 121 | Display(LiquidCrystal* lcd, const float& trigger_threshold): 122 | lcd_(lcd), 123 | trigger_threshold_(trigger_threshold), 124 | animation_(1000, 0.5) { 125 | elements_[HEADER] = Element{0, 0, 20}; 126 | elements_[VOLUME] = Element{1, 0, 11, "TV"}; 127 | elements_[BPM] = Element{2, 0, 11, "RR"}; 128 | elements_[IE_RATIO] = Element{3, 0, 11, "IE"}; 129 | elements_[AC_TRIGGER] = Element{0, 0, 11, "AC"}; 130 | elements_[PRES_LABEL] = Element{0, 11, 9}; 131 | elements_[PEAK_PRES] = Element{1, 11, 9, "peak"}; 132 | elements_[PLATEAU_PRES] = Element{2, 11, 9, "plat"}; 133 | elements_[PEEP_PRES] = Element{3, 11, 9, "PEEP"}; 134 | } 135 | 136 | // Setup during arduino setup() 137 | void begin(); 138 | 139 | // Update during arduino loop() 140 | void update(); 141 | 142 | // Write arbitrary alarm in the header 143 | void setAlarmText(const String& alarm); 144 | 145 | // Write value corresponding to key'ed element 146 | template 147 | void write(const DisplayKey& key, const T& value); 148 | 149 | // Write blank in the space corresponding to `key` 150 | void writeBlank(const DisplayKey& key); 151 | 152 | // Write current header 153 | void writeHeader(); 154 | 155 | // Write volume in mL 156 | void writeVolume(const int& vol); 157 | 158 | // Beats per minute 159 | void writeBPM(const int& bpm); 160 | 161 | // Inhale/exhale ratio in format 1:ie 162 | void writeIEratio(const float& ie); 163 | 164 | // AC trigger pressure 165 | void writeACTrigger(const float& ac_trigger); 166 | 167 | // Label for pressure units 168 | void writePresLabel(); 169 | 170 | // Peak pressure in cm of H2O 171 | void writePeakP(const int& peak); 172 | 173 | // Plateau pressure in cm of H2O 174 | void writePlateauP(const int& plat); 175 | 176 | // PEEP pressure in cm of H2O 177 | void writePEEP(const int& peep); 178 | 179 | // Convert value e.g. RR from numeric to string for displaying. 180 | template 181 | String toString(const DisplayKey& key, const T& value) const; 182 | 183 | // Get label of given element (empty string for elements without label, e.g. HEADER) 184 | inline String getLabel(const DisplayKey& key) const { return elements_[key].label; }; 185 | 186 | private: 187 | LiquidCrystal* lcd_; 188 | const float trigger_threshold_; 189 | TextAnimation animation_; 190 | Element elements_[NUM_KEYS]; 191 | 192 | // Write printable starting at (row, col) 193 | template 194 | void write(const int& row, const int& col, const T& printable); 195 | 196 | inline bool alarmsON() const { return !animation_.empty(); } 197 | }; 198 | 199 | 200 | // Instantiation of template methods 201 | #define INSTANTIATE_WRITE(type) \ 202 | template void Display::write(const DisplayKey& key, const type& value); 203 | INSTANTIATE_WRITE(int) 204 | INSTANTIATE_WRITE(float) 205 | #undef INSTANTIATE 206 | 207 | 208 | } // namespace display 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/examples/PacketSerialEncoderBareMinimum/PacketSerialEncoderBareMinimum.ino: -------------------------------------------------------------------------------- 1 | /* Library functions 2 | RoboClaw(uint8_t receivePin, uint8_t transmitPin, uint32_t tout); 3 | 4 | bool ForwardM1(uint8_t address, uint8_t speed); 5 | bool BackwardM1(uint8_t address, uint8_t speed); 6 | bool SetMinVoltageMainBattery(uint8_t address, uint8_t voltage); 7 | bool SetMaxVoltageMainBattery(uint8_t address, uint8_t voltage); 8 | bool ForwardM2(uint8_t address, uint8_t speed); 9 | bool BackwardM2(uint8_t address, uint8_t speed); 10 | bool ForwardBackwardM1(uint8_t address, uint8_t speed); 11 | bool ForwardBackwardM2(uint8_t address, uint8_t speed); 12 | bool ForwardMixed(uint8_t address, uint8_t speed); 13 | bool BackwardMixed(uint8_t address, uint8_t speed); 14 | bool TurnRightMixed(uint8_t address, uint8_t speed); 15 | bool TurnLeftMixed(uint8_t address, uint8_t speed); 16 | bool ForwardBackwardMixed(uint8_t address, uint8_t speed); 17 | bool LeftRightMixed(uint8_t address, uint8_t speed); 18 | uint32_t ReadEncM1(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 19 | uint32_t ReadEncM2(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 20 | bool SetEncM1(uint8_t address, int32_t val); 21 | bool SetEncM2(uint8_t address, int32_t val); 22 | uint32_t ReadSpeedM1(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 23 | uint32_t ReadSpeedM2(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 24 | bool ResetEncoders(uint8_t address); 25 | bool ReadVersion(uint8_t address,char *version); 26 | uint16_t ReadMainBatteryVoltage(uint8_t address,bool *valid=NULL); 27 | uint16_t ReadLogicBatteryVoltage(uint8_t address,bool *valid=NULL); 28 | bool SetMinVoltageLogicBattery(uint8_t address, uint8_t voltage); 29 | bool SetMaxVoltageLogicBattery(uint8_t address, uint8_t voltage); 30 | bool SetM1VelocityPID(uint8_t address, float Kp, float Ki, float Kd, uint32_t qpps); 31 | bool SetM2VelocityPID(uint8_t address, float Kp, float Ki, float Kd, uint32_t qpps); 32 | uint32_t ReadISpeedM1(uint8_t address,uint8_t *status=NULL,bool *valid=NULL); 33 | uint32_t ReadISpeedM2(uint8_t address,uint8_t *status=NULL,bool *valid=NULL); 34 | bool DutyM1(uint8_t address, uint16_t duty); 35 | bool DutyM2(uint8_t address, uint16_t duty); 36 | bool DutyM1M2(uint8_t address, uint16_t duty1, uint16_t duty2); 37 | bool SpeedM1(uint8_t address, uint32_t speed); 38 | bool SpeedM2(uint8_t address, uint32_t speed); 39 | bool SpeedM1M2(uint8_t address, uint32_t speed1, uint32_t speed2); 40 | bool SpeedAccelM1(uint8_t address, uint32_t accel, uint32_t speed); 41 | bool SpeedAccelM2(uint8_t address, uint32_t accel, uint32_t speed); 42 | bool SpeedAccelM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t speed2); 43 | bool SpeedDistanceM1(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag=0); 44 | bool SpeedDistanceM2(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag=0); 45 | bool SpeedDistanceM1M2(uint8_t address, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 46 | bool SpeedAccelDistanceM1(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag=0); 47 | bool SpeedAccelDistanceM2(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag=0); 48 | bool SpeedAccelDistanceM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 49 | bool ReadBuffers(uint8_t address, uint8_t &depth1, uint8_t &depth2); 50 | bool ReadPWMs(uint8_t address, int16_t &pwm1, int16_t &pwm2); 51 | bool ReadCurrents(uint8_t address, int16_t ¤t1, int16_t ¤t2); 52 | bool SpeedAccelM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t accel2, uint32_t speed2); 53 | bool SpeedAccelDistanceM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t distance1, uint32_t accel2, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 54 | bool DutyAccelM1(uint8_t address, uint16_t duty, uint32_t accel); 55 | bool DutyAccelM2(uint8_t address, uint16_t duty, uint32_t accel); 56 | bool DutyAccelM1M2(uint8_t address, uint16_t duty1, uint32_t accel1, uint16_t duty2, uint32_t accel2); 57 | bool ReadM1VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps); 58 | bool ReadM2VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps); 59 | bool SetMainVoltages(uint8_t address,uint16_t min,uint16_t max); 60 | bool SetLogicVoltages(uint8_t address,uint16_t min,uint16_t max); 61 | bool ReadMinMaxMainVoltages(uint8_t address,uint16_t &min,uint16_t &max); 62 | bool ReadMinMaxLogicVoltages(uint8_t address,uint16_t &min,uint16_t &max); 63 | bool SetM1PositionPID(uint8_t address,float kp,float ki,float kd,float kiMax,uint32_t deadzone,uint32_t min,uint32_t max); 64 | bool SetM2PositionPID(uint8_t address,float kp,float ki,float kd,float kiMax,uint32_t deadzone,uint32_t min,uint32_t max); 65 | bool ReadM1PositionPID(uint8_t address,float &Kp,float &Ki,float &Kd,float &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max); 66 | bool ReadM2PositionPID(uint8_t address,float &Kp,float &Ki,float &Kd,float &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max); 67 | bool SpeedAccelDeccelPositionM1(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag); 68 | bool SpeedAccelDeccelPositionM2(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag); 69 | bool SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1,uint32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2,uint32_t position2,uint8_t flag); 70 | bool SetM1DefaultAccel(uint8_t address, uint32_t accel); 71 | bool SetM2DefaultAccel(uint8_t address, uint32_t accel); 72 | bool SetPinFunctions(uint8_t address, uint8_t S3mode, uint8_t S4mode, uint8_t S5mode); 73 | bool GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode); 74 | bool SetDeadBand(uint8_t address, uint8_t Min, uint8_t Max); 75 | bool GetDeadBand(uint8_t address, uint8_t &Min, uint8_t &Max); 76 | bool RestoreDefaults(uint8_t address); 77 | bool ReadTemp(uint8_t address, uint16_t &temp); 78 | bool ReadTemp2(uint8_t address, uint16_t &temp); 79 | uint16_t ReadError(uint8_t address,bool *valid=NULL); 80 | bool ReadEncoderModes(uint8_t address, uint8_t &M1mode, uint8_t &M2mode); 81 | bool SetM1EncoderMode(uint8_t address,uint8_t mode); 82 | bool SetM2EncoderMode(uint8_t address,uint8_t mode); 83 | bool WriteNVM(uint8_t address); 84 | bool ReadNVM(uint8_t address); 85 | bool SetConfig(uint8_t address, uint16_t config); 86 | bool GetConfig(uint8_t address, uint16_t &config); 87 | bool SetM1MaxCurrent(uint8_t address,uint32_t max); 88 | bool SetM2MaxCurrent(uint8_t address,uint32_t max); 89 | bool ReadM1MaxCurrent(uint8_t address,uint32_t &max); 90 | bool ReadM2MaxCurrent(uint8_t address,uint32_t &max); 91 | bool SetPWMMode(uint8_t address, uint8_t mode); 92 | bool GetPWMMode(uint8_t address, uint8_t &mode); 93 | */ 94 | 95 | //Includes required to use Roboclaw library 96 | #include 97 | #include "RoboClaw.h" 98 | 99 | //Uncomment if Using Hardware Serial port 100 | //RoboClaw roboclaw(&Serial,10000); 101 | 102 | //Uncomment if using SoftwareSerial. See limitations of Arduino SoftwareSerial 103 | //SoftwareSerial serial(10,11); 104 | //RoboClaw roboclaw(&serial,10000); 105 | 106 | #define address 0x80 107 | 108 | void setup() { 109 | //Communicate at 38400bps 110 | roboclaw.begin(38400); 111 | } 112 | 113 | void loop() 114 | { 115 | } 116 | -------------------------------------------------------------------------------- /Alarms.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * Alarms.h 29 | * Defines classes for managing alarms, displaying alarm information on the display, 30 | * and playing tones corresponding to different alarm levels. 31 | */ 32 | 33 | #ifndef Alarms_h 34 | #define Alarms_h 35 | 36 | #include "Arduino.h" 37 | 38 | #include "Buttons.h" 39 | #include "Display.h" 40 | #include "pitches.h" 41 | 42 | 43 | namespace alarms { 44 | 45 | 46 | using display::Display; 47 | 48 | 49 | // Alarm levels in order of increasing priority 50 | enum AlarmLevel { 51 | NO_ALARM, 52 | NOTIFY, 53 | EMERGENCY, 54 | OFF_LEVEL, 55 | NUM_LEVELS 56 | }; 57 | 58 | 59 | // Container for notes elements 60 | struct Note { 61 | int note; 62 | int duration; 63 | int pause; 64 | }; 65 | 66 | 67 | // Notifiation notes 68 | static const Note kNotifyNotes[] = { 69 | {NOTE_B4, 200, 100}, 70 | {NOTE_B4, 200, 2000} 71 | }; 72 | 73 | // Emergency notes 74 | static const Note kEmergencyNotes[] = { 75 | {NOTE_G4, 300, 200}, 76 | {NOTE_G4, 300, 200}, 77 | {NOTE_G4, 300, 400}, 78 | {NOTE_G4, 200, 100}, 79 | {NOTE_G5, 200, 1500} 80 | }; 81 | 82 | // Notifiation notes 83 | static const Note kOffNotes[] = { 84 | {NOTE_G4, 200, 200} 85 | }; 86 | 87 | 88 | /** 89 | * Tone 90 | * A sequence of notes that can be played. 91 | */ 92 | class Tone { 93 | public: 94 | Tone(): length_(0) {} 95 | 96 | Tone(const Note notes[], const int& notes_length, const int* pin); 97 | 98 | // Play the tone, if any 99 | void play(); 100 | 101 | // Stop playing 102 | inline void stop() { playing_ = false; } 103 | 104 | private: 105 | Note* notes_; 106 | int length_; 107 | int* pin_; 108 | bool playing_ = false; 109 | int tone_step_; 110 | unsigned long tone_timer_ = 0; 111 | }; 112 | 113 | 114 | /** 115 | * Beeper 116 | * Represents the alarm speaker/buzzer. Handles playing of tones and snoozing. 117 | */ 118 | class Beeper { 119 | 120 | // Time during which alarms are silenced, in milliseconds 121 | static const unsigned long kSnoozeTime = 2 * 60 * 1000UL; 122 | 123 | public: 124 | Beeper(const int& beeper_pin, const int& snooze_pin): 125 | beeper_pin_(beeper_pin), 126 | snooze_button_(snooze_pin) { 127 | const int notify_notes_length = sizeof(kNotifyNotes) / sizeof(kNotifyNotes[0]); 128 | tones_[NOTIFY] = Tone(kNotifyNotes, notify_notes_length, &beeper_pin_); 129 | 130 | const int emergency_notes_length = sizeof(kEmergencyNotes) / sizeof(kEmergencyNotes[0]); 131 | tones_[EMERGENCY] = Tone(kEmergencyNotes, emergency_notes_length, &beeper_pin_); 132 | 133 | const int off_notes_length = sizeof(kOffNotes) / sizeof(kOffNotes[0]); 134 | tones_[OFF_LEVEL] = Tone(kOffNotes, off_notes_length, &beeper_pin_); 135 | } 136 | 137 | // Setup during arduino setup() 138 | void begin(); 139 | 140 | // Update during arduino loop() 141 | void update(const AlarmLevel& alarm_level); 142 | 143 | private: 144 | const int beeper_pin_; 145 | buttons::DebouncedButton snooze_button_; 146 | Tone tones_[NUM_LEVELS]; 147 | 148 | unsigned long snooze_time_ = 0; 149 | bool snoozed_ = false; 150 | 151 | bool snoozeButtonPressed() const; 152 | 153 | void toggleSnooze(); 154 | 155 | void play(const AlarmLevel& alarm_level); 156 | 157 | void stop(); 158 | }; 159 | 160 | 161 | /** 162 | * Alarm 163 | * Keeps track of the state of a specific alarm. 164 | */ 165 | class Alarm { 166 | public: 167 | Alarm() {}; 168 | 169 | Alarm(const String& default_text, const int& min_bad_to_trigger, 170 | const int& min_good_to_clear, const AlarmLevel& alarm_level); 171 | 172 | // Reset to default state 173 | void reset(); 174 | 175 | // Set the ON value of this alarm, but only turn ON if `bad == true` for at least 176 | // `min_bad_to_trigger_` consecutive calls with different `seq` and OFF if `bad == false` 177 | // for at least `min_good_to_clear_` consecutive calls with different `seq`. 178 | void setCondition(const bool& bad, const unsigned long& seq); 179 | 180 | // Set the alarm text (trim or pad to display width) 181 | void setText(const String& text); 182 | 183 | // Check if this alarm is on 184 | inline const bool& isON() const { return on_; } 185 | 186 | // Get the text of this alarm 187 | inline String text() const { return text_; } 188 | 189 | // Get the alarm level of this alarm 190 | inline AlarmLevel alarmLevel() const { return alarm_level_; } 191 | 192 | private: 193 | String text_; 194 | AlarmLevel alarm_level_; 195 | int min_bad_to_trigger_; 196 | int min_good_to_clear_; 197 | bool on_ = false; 198 | unsigned long consecutive_bad_ = 0; 199 | unsigned long consecutive_good_ = 0; 200 | unsigned long last_bad_seq_ = 0; 201 | unsigned long last_good_seq_ = 0; 202 | }; 203 | 204 | 205 | /** 206 | * AlarmManager 207 | * Manages multple alarms on the same screen space. 208 | * If there is one alarm on, its text blinks in a designated portion of the screen, 209 | * if there are more, each one blinks for `kDisplayTime` milliseconds at a time. 210 | */ 211 | class AlarmManager { 212 | 213 | // Time each alarm is displayed if multiple, in milliseconds 214 | static const unsigned long kDisplayTime = 2 * 1000UL; 215 | 216 | // Indices for the different alarms 217 | enum Indices { 218 | HIGH_PRESSU = 0, 219 | LOW_PRESSUR, 220 | BAD_PLATEAU, 221 | UNMET_VOLUM, 222 | NO_TIDAL_PR, 223 | OVER_CURREN, 224 | MECH_FAILUR, 225 | NOT_CONFIRM, 226 | TURNING_OFF, 227 | NUM_ALARMS 228 | }; 229 | 230 | public: 231 | AlarmManager(const int& beeper_pin, const int& snooze_pin, const int& led_pin, 232 | Display* displ, unsigned long const* cycle_count): 233 | displ_(displ), 234 | beeper_(beeper_pin, snooze_pin), 235 | led_pin_(led_pin), 236 | led_pulse_(500, 0.5), 237 | cycle_count_(cycle_count) { 238 | alarms_[HIGH_PRESSU] = Alarm(" HIGH PRESSURE ", 1, 2, EMERGENCY); 239 | alarms_[LOW_PRESSUR] = Alarm("LOW PRES DISCONNECT?", 1, 1, EMERGENCY); 240 | alarms_[BAD_PLATEAU] = Alarm(" HIGH RESIST PRES ", 1, 1, NOTIFY); 241 | alarms_[UNMET_VOLUM] = Alarm(" UNMET TIDAL VOLUME ", 1, 1, EMERGENCY); 242 | alarms_[NO_TIDAL_PR] = Alarm(" NO TIDAL PRESSURE ", 2, 1, EMERGENCY); 243 | alarms_[OVER_CURREN] = Alarm(" OVER CURRENT FAULT ", 1, 2, EMERGENCY); 244 | alarms_[MECH_FAILUR] = Alarm(" MECHANICAL FAILURE ", 1, 1, EMERGENCY); 245 | alarms_[NOT_CONFIRM] = Alarm(" CONFIRM? ", 1, 1, NOTIFY); 246 | alarms_[TURNING_OFF] = Alarm(" TURNING OFF ", 1, 1, OFF_LEVEL); 247 | } 248 | 249 | // Setup during arduino setup() 250 | void begin(); 251 | 252 | // Update alarms, should be called every loop 253 | void update(); 254 | 255 | // Clear all alarms 256 | void allOff(); 257 | 258 | // Pressure too high alarm 259 | inline void highPressure(const bool& value) { 260 | alarms_[HIGH_PRESSU].setCondition(value, *cycle_count_); 261 | } 262 | 263 | // Pressure too low alarm 264 | inline void lowPressure(const bool& value) { 265 | alarms_[LOW_PRESSUR].setCondition(value, *cycle_count_); 266 | } 267 | 268 | // Bad plateau alarm 269 | inline void badPlateau(const bool& value) { 270 | alarms_[BAD_PLATEAU].setCondition(value, *cycle_count_); 271 | } 272 | 273 | // Tidal volume not met alarm 274 | inline void unmetVolume(const bool& value) { 275 | alarms_[UNMET_VOLUM].setCondition(value, *cycle_count_); 276 | } 277 | 278 | // Tidal pressure not detected alarm 279 | inline void noTidalPres(const bool& value) { 280 | alarms_[NO_TIDAL_PR].setCondition(value, *cycle_count_); 281 | } 282 | 283 | // Current too high alarm 284 | inline void overCurrent(const bool& value) { 285 | alarms_[OVER_CURREN].setCondition(value, *cycle_count_); 286 | } 287 | 288 | // Mechanical Failure alarm 289 | inline void mechanicalFailure(const bool& value) { 290 | alarms_[MECH_FAILUR].setCondition(value, *cycle_count_); 291 | } 292 | 293 | // Setting not confirmed 294 | inline void unconfirmedChange(const bool& value, const String& message = "") { 295 | if (value) { 296 | alarms_[NOT_CONFIRM].setText(message); 297 | } 298 | alarms_[NOT_CONFIRM].setCondition(value, *cycle_count_); 299 | } 300 | 301 | inline void turningOFF(const bool& value) { 302 | alarms_[TURNING_OFF].setCondition(value, *cycle_count_); 303 | } 304 | 305 | // Get current state of each alarm 306 | inline const bool& getHighPressure() { return alarms_[HIGH_PRESSU].isON(); } 307 | inline const bool& getLowPressure() { return alarms_[LOW_PRESSUR].isON(); } 308 | inline const bool& getBadPlateau() { return alarms_[BAD_PLATEAU].isON(); } 309 | inline const bool& getUnmetVolume() { return alarms_[UNMET_VOLUM].isON(); } 310 | inline const bool& getNoTidalPres() { return alarms_[NO_TIDAL_PR].isON(); } 311 | inline const bool& getOverCurrent() { return alarms_[OVER_CURREN].isON(); } 312 | inline const bool& getMechanicalFailure() { return alarms_[MECH_FAILUR].isON(); } 313 | inline const bool& getUnconfirmedChange() { return alarms_[NOT_CONFIRM].isON(); } 314 | inline const bool& getTurningOFF() { return alarms_[TURNING_OFF].isON(); } 315 | 316 | private: 317 | Display* displ_; 318 | Beeper beeper_; 319 | int led_pin_; 320 | utils::Pulse led_pulse_; 321 | Alarm alarms_[NUM_ALARMS]; 322 | unsigned long const* cycle_count_; 323 | 324 | // Get number of alarms that are ON 325 | int numON() const; 326 | 327 | // Get text to display 328 | String getText() const; 329 | 330 | // Get highest priority level of the alarms that are ON 331 | AlarmLevel getHighestLevel() const; 332 | }; 333 | 334 | 335 | } // namespace alarms 336 | 337 | 338 | #endif 339 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/RoboClaw.h: -------------------------------------------------------------------------------- 1 | #ifndef RoboClaw_h 2 | #define RoboClaw_h 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #ifdef __AVR__ 10 | #include 11 | #endif 12 | 13 | /****************************************************************************** 14 | * Definitions 15 | ******************************************************************************/ 16 | 17 | #define _RC_VERSION 10 // software version of this library 18 | #ifndef GCC_VERSION 19 | #define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) 20 | #endif 21 | 22 | #define _SS_VERSION 16 23 | 24 | class RoboClaw : public Stream 25 | { 26 | uint16_t crc; 27 | uint32_t timeout; 28 | 29 | HardwareSerial *hserial; 30 | #ifdef __AVR__ 31 | SoftwareSerial *sserial; 32 | #endif 33 | 34 | enum {M1FORWARD = 0, 35 | M1BACKWARD = 1, 36 | SETMINMB = 2, 37 | SETMAXMB = 3, 38 | M2FORWARD = 4, 39 | M2BACKWARD = 5, 40 | M17BIT = 6, 41 | M27BIT = 7, 42 | MIXEDFORWARD = 8, 43 | MIXEDBACKWARD = 9, 44 | MIXEDRIGHT = 10, 45 | MIXEDLEFT = 11, 46 | MIXEDFB = 12, 47 | MIXEDLR = 13, 48 | GETM1ENC = 16, 49 | GETM2ENC = 17, 50 | GETM1SPEED = 18, 51 | GETM2SPEED = 19, 52 | RESETENC = 20, 53 | GETVERSION = 21, 54 | SETM1ENCCOUNT = 22, 55 | SETM2ENCCOUNT = 23, 56 | GETMBATT = 24, 57 | GETLBATT = 25, 58 | SETMINLB = 26, 59 | SETMAXLB = 27, 60 | SETM1PID = 28, 61 | SETM2PID = 29, 62 | GETM1ISPEED = 30, 63 | GETM2ISPEED = 31, 64 | M1DUTY = 32, 65 | M2DUTY = 33, 66 | MIXEDDUTY = 34, 67 | M1SPEED = 35, 68 | M2SPEED = 36, 69 | MIXEDSPEED = 37, 70 | M1SPEEDACCEL = 38, 71 | M2SPEEDACCEL = 39, 72 | MIXEDSPEEDACCEL = 40, 73 | M1SPEEDDIST = 41, 74 | M2SPEEDDIST = 42, 75 | MIXEDSPEEDDIST = 43, 76 | M1SPEEDACCELDIST = 44, 77 | M2SPEEDACCELDIST = 45, 78 | MIXEDSPEEDACCELDIST = 46, 79 | GETBUFFERS = 47, 80 | GETPWMS = 48, 81 | GETCURRENTS = 49, 82 | MIXEDSPEED2ACCEL = 50, 83 | MIXEDSPEED2ACCELDIST = 51, 84 | M1DUTYACCEL = 52, 85 | M2DUTYACCEL = 53, 86 | MIXEDDUTYACCEL = 54, 87 | READM1PID = 55, 88 | READM2PID = 56, 89 | SETMAINVOLTAGES = 57, 90 | SETLOGICVOLTAGES = 58, 91 | GETMINMAXMAINVOLTAGES = 59, 92 | GETMINMAXLOGICVOLTAGES = 60, 93 | SETM1POSPID = 61, 94 | SETM2POSPID = 62, 95 | READM1POSPID = 63, 96 | READM2POSPID = 64, 97 | M1SPEEDACCELDECCELPOS = 65, 98 | M2SPEEDACCELDECCELPOS = 66, 99 | MIXEDSPEEDACCELDECCELPOS = 67, 100 | SETM1DEFAULTACCEL = 68, 101 | SETM2DEFAULTACCEL = 69, 102 | SETPINFUNCTIONS = 74, 103 | GETPINFUNCTIONS = 75, 104 | SETDEADBAND = 76, 105 | GETDEADBAND = 77, 106 | GETENCODERS = 78, 107 | GETISPEEDS = 79, 108 | RESTOREDEFAULTS = 80, 109 | GETTEMP = 82, 110 | GETTEMP2 = 83, //Only valid on some models 111 | GETERROR = 90, 112 | GETENCODERMODE = 91, 113 | SETM1ENCODERMODE = 92, 114 | SETM2ENCODERMODE = 93, 115 | WRITENVM = 94, 116 | READNVM = 95, //Reloads values from Flash into Ram 117 | SETCONFIG = 98, 118 | GETCONFIG = 99, 119 | SETM1MAXCURRENT = 133, 120 | SETM2MAXCURRENT = 134, 121 | GETM1MAXCURRENT = 135, 122 | GETM2MAXCURRENT = 136, 123 | SETPWMMODE = 148, 124 | GETPWMMODE = 149, 125 | FLAGBOOTLOADER = 255}; //Only available via USB communications 126 | public: 127 | // public methods 128 | RoboClaw(HardwareSerial *hserial,uint32_t tout); 129 | #ifdef __AVR__ 130 | RoboClaw(SoftwareSerial *sserial,uint32_t tout); 131 | #endif 132 | 133 | ~RoboClaw(); 134 | 135 | bool ForwardM1(uint8_t address, uint8_t speed); 136 | bool BackwardM1(uint8_t address, uint8_t speed); 137 | bool SetMinVoltageMainBattery(uint8_t address, uint8_t voltage); 138 | bool SetMaxVoltageMainBattery(uint8_t address, uint8_t voltage); 139 | bool ForwardM2(uint8_t address, uint8_t speed); 140 | bool BackwardM2(uint8_t address, uint8_t speed); 141 | bool ForwardBackwardM1(uint8_t address, uint8_t speed); 142 | bool ForwardBackwardM2(uint8_t address, uint8_t speed); 143 | bool ForwardMixed(uint8_t address, uint8_t speed); 144 | bool BackwardMixed(uint8_t address, uint8_t speed); 145 | bool TurnRightMixed(uint8_t address, uint8_t speed); 146 | bool TurnLeftMixed(uint8_t address, uint8_t speed); 147 | bool ForwardBackwardMixed(uint8_t address, uint8_t speed); 148 | bool LeftRightMixed(uint8_t address, uint8_t speed); 149 | uint32_t ReadEncM1(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 150 | uint32_t ReadEncM2(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 151 | bool SetEncM1(uint8_t address, int32_t val); 152 | bool SetEncM2(uint8_t address, int32_t val); 153 | uint32_t ReadSpeedM1(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 154 | uint32_t ReadSpeedM2(uint8_t address, uint8_t *status=NULL,bool *valid=NULL); 155 | bool ResetEncoders(uint8_t address); 156 | bool ReadVersion(uint8_t address,char *version); 157 | uint16_t ReadMainBatteryVoltage(uint8_t address,bool *valid=NULL); 158 | uint16_t ReadLogicBatteryVoltage(uint8_t address,bool *valid=NULL); 159 | bool SetMinVoltageLogicBattery(uint8_t address, uint8_t voltage); 160 | bool SetMaxVoltageLogicBattery(uint8_t address, uint8_t voltage); 161 | bool SetM1VelocityPID(uint8_t address, float Kp, float Ki, float Kd, uint32_t qpps); 162 | bool SetM2VelocityPID(uint8_t address, float Kp, float Ki, float Kd, uint32_t qpps); 163 | uint32_t ReadISpeedM1(uint8_t address,uint8_t *status=NULL,bool *valid=NULL); 164 | uint32_t ReadISpeedM2(uint8_t address,uint8_t *status=NULL,bool *valid=NULL); 165 | bool DutyM1(uint8_t address, uint16_t duty); 166 | bool DutyM2(uint8_t address, uint16_t duty); 167 | bool DutyM1M2(uint8_t address, uint16_t duty1, uint16_t duty2); 168 | bool SpeedM1(uint8_t address, uint32_t speed); 169 | bool SpeedM2(uint8_t address, uint32_t speed); 170 | bool SpeedM1M2(uint8_t address, uint32_t speed1, uint32_t speed2); 171 | bool SpeedAccelM1(uint8_t address, uint32_t accel, uint32_t speed); 172 | bool SpeedAccelM2(uint8_t address, uint32_t accel, uint32_t speed); 173 | bool SpeedAccelM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t speed2); 174 | bool SpeedDistanceM1(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag=0); 175 | bool SpeedDistanceM2(uint8_t address, uint32_t speed, uint32_t distance, uint8_t flag=0); 176 | bool SpeedDistanceM1M2(uint8_t address, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 177 | bool SpeedAccelDistanceM1(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag=0); 178 | bool SpeedAccelDistanceM2(uint8_t address, uint32_t accel, uint32_t speed, uint32_t distance, uint8_t flag=0); 179 | bool SpeedAccelDistanceM1M2(uint8_t address, uint32_t accel, uint32_t speed1, uint32_t distance1, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 180 | bool ReadBuffers(uint8_t address, uint8_t &depth1, uint8_t &depth2); 181 | bool ReadPWMs(uint8_t address, int16_t &pwm1, int16_t &pwm2); 182 | bool ReadCurrents(uint8_t address, int16_t ¤t1, int16_t ¤t2); 183 | bool SpeedAccelM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t accel2, uint32_t speed2); 184 | bool SpeedAccelDistanceM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t distance1, uint32_t accel2, uint32_t speed2, uint32_t distance2, uint8_t flag=0); 185 | bool DutyAccelM1(uint8_t address, uint16_t duty, uint32_t accel); 186 | bool DutyAccelM2(uint8_t address, uint16_t duty, uint32_t accel); 187 | bool DutyAccelM1M2(uint8_t address, uint16_t duty1, uint32_t accel1, uint16_t duty2, uint32_t accel2); 188 | bool ReadM1VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps); 189 | bool ReadM2VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps); 190 | bool SetMainVoltages(uint8_t address,uint16_t min,uint16_t max); 191 | bool SetLogicVoltages(uint8_t address,uint16_t min,uint16_t max); 192 | bool ReadMinMaxMainVoltages(uint8_t address,uint16_t &min,uint16_t &max); 193 | bool ReadMinMaxLogicVoltages(uint8_t address,uint16_t &min,uint16_t &max); 194 | bool SetM1PositionPID(uint8_t address,float kp,float ki,float kd,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max); 195 | bool SetM2PositionPID(uint8_t address,float kp,float ki,float kd,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max); 196 | bool ReadM1PositionPID(uint8_t address,float &Kp,float &Ki,float &Kd,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max); 197 | bool ReadM2PositionPID(uint8_t address,float &Kp,float &Ki,float &Kd,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max); 198 | bool SpeedAccelDeccelPositionM1(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag); 199 | bool SpeedAccelDeccelPositionM2(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag); 200 | bool SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1,uint32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2,uint32_t position2,uint8_t flag); 201 | bool SetM1DefaultAccel(uint8_t address, uint32_t accel); 202 | bool SetM2DefaultAccel(uint8_t address, uint32_t accel); 203 | bool SetPinFunctions(uint8_t address, uint8_t S3mode, uint8_t S4mode, uint8_t S5mode); 204 | bool GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode); 205 | bool SetDeadBand(uint8_t address, uint8_t Min, uint8_t Max); 206 | bool GetDeadBand(uint8_t address, uint8_t &Min, uint8_t &Max); 207 | bool ReadEncoders(uint8_t address,uint32_t &enc1,uint32_t &enc2); 208 | bool ReadISpeeds(uint8_t address,uint32_t &ispeed1,uint32_t &ispeed2); 209 | bool RestoreDefaults(uint8_t address); 210 | bool ReadTemp(uint8_t address, uint16_t &temp); 211 | bool ReadTemp2(uint8_t address, uint16_t &temp); 212 | uint32_t ReadError(uint8_t address,bool *valid=NULL); 213 | bool ReadEncoderModes(uint8_t address, uint8_t &M1mode, uint8_t &M2mode); 214 | bool SetM1EncoderMode(uint8_t address,uint8_t mode); 215 | bool SetM2EncoderMode(uint8_t address,uint8_t mode); 216 | bool WriteNVM(uint8_t address); 217 | bool ReadNVM(uint8_t address); 218 | bool SetConfig(uint8_t address, uint16_t config); 219 | bool GetConfig(uint8_t address, uint16_t &config); 220 | bool SetM1MaxCurrent(uint8_t address,uint32_t max); 221 | bool SetM2MaxCurrent(uint8_t address,uint32_t max); 222 | bool ReadM1MaxCurrent(uint8_t address,uint32_t &max); 223 | bool ReadM2MaxCurrent(uint8_t address,uint32_t &max); 224 | bool SetPWMMode(uint8_t address, uint8_t mode); 225 | bool GetPWMMode(uint8_t address, uint8_t &mode); 226 | 227 | static int16_t library_version() { return _SS_VERSION; } 228 | 229 | virtual int available(); 230 | void begin(long speed); 231 | bool isListening(); 232 | bool overflow(); 233 | int peek(); 234 | virtual int read(); 235 | int read(uint32_t timeout); 236 | bool listen(); 237 | virtual size_t write(uint8_t byte); 238 | virtual void flush(); 239 | void clear(); 240 | 241 | private: 242 | void crc_clear(); 243 | void crc_update (uint8_t data); 244 | uint16_t crc_get(); 245 | bool write_n(uint8_t byte,...); 246 | bool read_n(uint8_t byte,uint8_t address,uint8_t cmd,...); 247 | uint32_t Read4_1(uint8_t address,uint8_t cmd,uint8_t *status,bool *valid); 248 | uint32_t Read4(uint8_t address,uint8_t cmd,bool *valid); 249 | uint16_t Read2(uint8_t address,uint8_t cmd,bool *valid); 250 | uint8_t Read1(uint8_t address,uint8_t cmd,bool *valid); 251 | 252 | }; 253 | 254 | #endif -------------------------------------------------------------------------------- /e-vent.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Emergency Ventilator Controller 3 | * 4 | * MIT License: 5 | * 6 | * Copyright (c) 2020 MIT 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in all 16 | * copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | * SOFTWARE. 25 | */ 26 | 27 | /** 28 | * e-vent.ino 29 | * Main Arduino file. 30 | */ 31 | 32 | #include "LiquidCrystal.h" 33 | #include "src/thirdparty/RoboClaw/RoboClaw.h" 34 | #include "cpp_utils.h" // Redefines macros min, max, abs, etc. into proper functions, 35 | // should be included after third-party code, before E-Vent includes 36 | #include "Alarms.h" 37 | #include "Buttons.h" 38 | #include "Constants.h" 39 | #include "Display.h" 40 | #include "Input.h" 41 | #include "Logging.h" 42 | #include "Pressure.h" 43 | 44 | 45 | using namespace input; 46 | using namespace utils; 47 | 48 | 49 | ///////////////////// 50 | // Initialize Vars // 51 | ///////////////////// 52 | 53 | // Cycle parameters 54 | unsigned long cycleCount = 0; 55 | float tCycleTimer; // Absolute time (s) at start of each breathing cycle 56 | float tIn; // Calculated time (s) since tCycleTimer for end of IN_STATE 57 | float tHoldIn; // Calculated time (s) since tCycleTimer for end of HOLD_IN_STATE 58 | float tEx; // Calculated time (s) since tCycleTimer for end of EX_STATE 59 | float tPeriod; // Calculated time (s) since tCycleTimer for end of cycle 60 | float tPeriodActual; // Actual time (s) since tCycleTimer at end of cycle (for logging) 61 | float tLoopTimer; // Absolute time (s) at start of each control loop iteration 62 | float tLoopBuffer; // Amount of time (s) left at end of each loop 63 | 64 | // States 65 | States state; 66 | bool enteringState; 67 | float tStateTimer; 68 | 69 | // Roboclaw 70 | RoboClaw roboclaw(&Serial3, 10000); 71 | int motorCurrent, motorPosition = 0; 72 | 73 | // LCD Screen 74 | LiquidCrystal lcd(LCD_RS_PIN, LCD_EN_PIN, LCD_D4_PIN, dLCD_D5_PIN, LCD_D6_PIN, LCD_D7_PIN); 75 | display::Display displ(&lcd, AC_MIN); 76 | 77 | // Alarms 78 | alarms::AlarmManager alarm(BEEPER_PIN, SNOOZE_PIN, LED_ALARM_PIN, &displ, &cycleCount); 79 | 80 | // Pressure 81 | Pressure pressureReader(PRESS_SENSE_PIN); 82 | 83 | // Buttons 84 | buttons::PressHoldButton offButton(OFF_PIN, 2000); 85 | buttons::DebouncedButton confirmButton(CONFIRM_PIN); 86 | 87 | // Logger 88 | logging::Logger logger(true/*Serial*/, false/*SD*/, false/*labels*/, ",\t"/*delim*/); 89 | 90 | // Knobs 91 | struct Knobs { 92 | int volume(); // Tidal volume 93 | int bpm(); // Respiratory rate 94 | float ie(); // Inhale/exhale ratio 95 | float ac(); // Assist control trigger sensitivity 96 | SafeKnob volume_ = SafeKnob(&displ, display::VOLUME, CONFIRM_PIN, &alarm, VOL_RES); 97 | SafeKnob bpm_ = SafeKnob(&displ, display::BPM, CONFIRM_PIN, &alarm, BPM_RES); 98 | SafeKnob ie_ = SafeKnob(&displ, display::IE_RATIO, CONFIRM_PIN, &alarm, IE_RES); 99 | SafeKnob ac_ = SafeKnob(&displ, display::AC_TRIGGER, CONFIRM_PIN, &alarm, AC_RES); 100 | void begin(); 101 | void update(); 102 | } knobs; 103 | 104 | // Assist control 105 | bool patientTriggered = false; 106 | 107 | 108 | /////////////////////// 109 | // Declare Functions // 110 | /////////////////////// 111 | 112 | // Set the current state in the state machine 113 | void setState(States newState); 114 | 115 | // Calculates the waveform parameters from the user inputs 116 | void calculateWaveform(); 117 | 118 | // Check for errors and take appropriate action 119 | void handleErrors(); 120 | 121 | // Set up logger variables 122 | void setupLogger(); 123 | 124 | 125 | /////////////////// 126 | ////// Setup ////// 127 | /////////////////// 128 | 129 | void setup() { 130 | Serial.begin(SERIAL_BAUD_RATE); 131 | while(!Serial); 132 | 133 | if (DEBUG) { 134 | setState(DEBUG_STATE); 135 | } else { 136 | setState(PREHOME_STATE); // Initial state 137 | } 138 | 139 | // Wait for the roboclaw to boot up 140 | delay(1000); 141 | 142 | //Initialize 143 | pinMode(HOME_PIN, INPUT_PULLUP); // Pull up the limit switch 144 | setupLogger(); 145 | alarm.begin(); 146 | displ.begin(); 147 | offButton.begin(); 148 | confirmButton.begin(); 149 | knobs.begin(); 150 | tCycleTimer = now(); 151 | 152 | roboclaw.begin(ROBOCLAW_BAUD); 153 | roboclaw.SetM1MaxCurrent(ROBOCLAW_ADDR, ROBOCLAW_MAX_CURRENT); 154 | roboclaw.SetM1VelocityPID(ROBOCLAW_ADDR, VKP, VKI, VKD, QPPS); 155 | roboclaw.SetM1PositionPID(ROBOCLAW_ADDR, PKP, PKI, PKD, KI_MAX, DEADZONE, MIN_POS, MAX_POS); 156 | roboclaw.SetEncM1(ROBOCLAW_ADDR, 0); // Zero the encoder 157 | } 158 | 159 | ////////////////// 160 | ////// Loop ////// 161 | ////////////////// 162 | 163 | void loop() { 164 | if (DEBUG) { 165 | if (Serial.available() > 0) { 166 | setState((States) Serial.parseInt()); 167 | while(Serial.available() > 0) Serial.read(); 168 | } 169 | } 170 | 171 | // All States 172 | tLoopTimer = now(); // Start the loop timer 173 | logger.update(); 174 | knobs.update(); 175 | calculateWaveform(); 176 | readEncoder(roboclaw, motorPosition); // TODO handle invalid reading 177 | readMotorCurrent(roboclaw, motorCurrent); 178 | pressureReader.read(); 179 | handleErrors(); 180 | alarm.update(); 181 | displ.update(); 182 | offButton.update(); 183 | 184 | if (offButton.wasHeld()) { 185 | goToPositionByDur(roboclaw, BAG_CLEAR_POS, motorPosition, MAX_EX_DURATION); 186 | setState(OFF_STATE); 187 | alarm.allOff(); 188 | } 189 | 190 | // State Machine 191 | switch (state) { 192 | 193 | case DEBUG_STATE: 194 | // Stop motor 195 | roboclaw.ForwardM1(ROBOCLAW_ADDR, 0); 196 | break; 197 | 198 | case OFF_STATE: 199 | alarm.turningOFF(now() - tStateTimer < TURNING_OFF_DURATION); 200 | if (confirmButton.is_LOW()) { 201 | setState(PREHOME_STATE); 202 | alarm.turningOFF(false); 203 | } 204 | break; 205 | 206 | case IN_STATE: 207 | if (enteringState) { 208 | enteringState = false; 209 | const float tNow = now(); 210 | tPeriodActual = tNow - tCycleTimer; 211 | tCycleTimer = tNow; // The cycle begins at the start of inspiration 212 | goToPositionByDur(roboclaw, volume2ticks(knobs.volume()), motorPosition, tIn); 213 | cycleCount++; 214 | } 215 | 216 | if (now() - tCycleTimer > tIn) { 217 | setState(HOLD_IN_STATE); 218 | } 219 | break; 220 | 221 | case HOLD_IN_STATE: 222 | if (enteringState) { 223 | enteringState = false; 224 | } 225 | if (now() - tCycleTimer > tHoldIn) { 226 | pressureReader.set_plateau(); 227 | setState(EX_STATE); 228 | } 229 | break; 230 | 231 | case EX_STATE: 232 | if (enteringState) { 233 | enteringState = false; 234 | goToPositionByDur(roboclaw, BAG_CLEAR_POS, motorPosition, tEx - (now() - tCycleTimer)); 235 | } 236 | 237 | if (abs(motorPosition - BAG_CLEAR_POS) < BAG_CLEAR_TOL) { 238 | setState(PEEP_PAUSE_STATE); 239 | } 240 | break; 241 | 242 | case PEEP_PAUSE_STATE: 243 | if (enteringState) { 244 | enteringState = false; 245 | } 246 | 247 | if (now() - tCycleTimer > tEx + MIN_PEEP_PAUSE) { 248 | pressureReader.set_peep(); 249 | 250 | setState(HOLD_EX_STATE); 251 | } 252 | break; 253 | 254 | case HOLD_EX_STATE: 255 | if (enteringState) { 256 | enteringState = false; 257 | } 258 | 259 | // Check if patient triggers inhale 260 | patientTriggered = pressureReader.get() < (pressureReader.peep() - knobs.ac()) 261 | && knobs.ac() > AC_MIN; 262 | 263 | if (patientTriggered || now() - tCycleTimer > tPeriod) { 264 | if (!patientTriggered) pressureReader.set_peep(); // Set peep again if time triggered 265 | pressureReader.set_peak_and_reset(); 266 | displ.writePeakP(round(pressureReader.peak())); 267 | displ.writePEEP(round(pressureReader.peep())); 268 | displ.writePlateauP(round(pressureReader.plateau())); 269 | setState(IN_STATE); 270 | } 271 | break; 272 | 273 | case PREHOME_STATE: 274 | if (enteringState) { 275 | enteringState = false; 276 | roboclaw.BackwardM1(ROBOCLAW_ADDR, HOMING_VOLTS); 277 | } 278 | 279 | if (homeSwitchPressed()) { 280 | setState(HOMING_STATE); 281 | } 282 | break; 283 | 284 | case HOMING_STATE: 285 | if (enteringState) { 286 | enteringState = false; 287 | roboclaw.ForwardM1(ROBOCLAW_ADDR, HOMING_VOLTS); 288 | } 289 | 290 | if (!homeSwitchPressed()) { 291 | roboclaw.ForwardM1(ROBOCLAW_ADDR, 0); 292 | delay(HOMING_PAUSE * 1000); // Wait for things to settle 293 | roboclaw.SetEncM1(ROBOCLAW_ADDR, 0); // Zero the encoder 294 | setState(IN_STATE); 295 | } 296 | break; 297 | } 298 | 299 | // Add a delay if there's still time in the loop period 300 | tLoopBuffer = max(0, tLoopTimer + LOOP_PERIOD - now()); 301 | delay(tLoopBuffer*1000.0); 302 | } 303 | 304 | 305 | ///////////////// 306 | // Definitions // 307 | ///////////////// 308 | 309 | void Knobs::begin() { 310 | volume_.begin(&readVolume); 311 | bpm_.begin(&readBpm); 312 | ie_.begin(&readIeRatio); 313 | ac_.begin(&readAc); 314 | } 315 | 316 | void Knobs::update() { 317 | volume_.update(); 318 | bpm_.update(); 319 | ie_.update(); 320 | ac_.update(); 321 | } 322 | 323 | inline int Knobs::volume() { return volume_.read(); } 324 | inline int Knobs::bpm() { return bpm_.read(); } 325 | inline float Knobs::ie() { return ie_.read(); } 326 | inline float Knobs::ac() { return ac_.read(); } 327 | 328 | void setState(States newState) { 329 | enteringState = true; 330 | state = newState; 331 | tStateTimer = now(); 332 | } 333 | 334 | void calculateWaveform() { 335 | tPeriod = 60.0 / knobs.bpm(); // seconds in each breathing cycle period 336 | tHoldIn = tPeriod / (1 + knobs.ie()); 337 | tIn = tHoldIn - HOLD_IN_DURATION; 338 | tEx = min(tHoldIn + MAX_EX_DURATION, tPeriod - MIN_PEEP_PAUSE); 339 | } 340 | 341 | void handleErrors() { 342 | // Pressure alarms 343 | const bool over_pressure = pressureReader.get() >= MAX_PRESSURE; 344 | alarm.highPressure(over_pressure); 345 | if (over_pressure) setState(EX_STATE); 346 | 347 | // These pressure alarms only make sense after homing 348 | if (enteringState && state == IN_STATE) { 349 | alarm.badPlateau(pressureReader.peak() - pressureReader.plateau() > MAX_RESIST_PRESSURE); 350 | alarm.lowPressure(pressureReader.plateau() < MIN_PLATEAU_PRESSURE); 351 | alarm.noTidalPres(pressureReader.peak() - pressureReader.peep() < MIN_TIDAL_PRESSURE); 352 | } 353 | 354 | // Check if desired volume was reached 355 | if (enteringState && state == EX_STATE) { 356 | alarm.unmetVolume(knobs.volume() - ticks2volume(motorPosition) > VOLUME_ERROR_THRESH); 357 | } 358 | 359 | // Check if maximum motor current was exceeded 360 | if (motorCurrent >= MAX_MOTOR_CURRENT) { 361 | setState(EX_STATE); 362 | alarm.overCurrent(true); 363 | } else { 364 | alarm.overCurrent(false); 365 | } 366 | 367 | // Check if we've gotten stuck in EX_STATE (mechanical cycle didn't finsih) 368 | alarm.mechanicalFailure(state == EX_STATE && now() - tCycleTimer > tPeriod + MECHANICAL_TIMEOUT); 369 | } 370 | 371 | void setupLogger() { 372 | logger.addVar("Time", &tLoopTimer); 373 | logger.addVar("CycleStart", &tCycleTimer); 374 | logger.addVar("State", (int*)&state); 375 | logger.addVar("Pos", &motorPosition, 3); 376 | logger.addVar("Pressure", &pressureReader.get(), 6); 377 | // logger.addVar("Period", &tPeriodActual); 378 | // logger.addVar("tLoopBuffer", &tLoopBuffer, 6, 4); 379 | // logger.addVar("Current", &motorCurrent, 3); 380 | // logger.addVar("Peep", &pressureReader.peep(), 6); 381 | // logger.addVar("HighPresAlarm", &alarm.getHighPressure()); 382 | // begin called after all variables added to include them all in the header 383 | logger.begin(&Serial, SD_SELECT); 384 | } 385 | -------------------------------------------------------------------------------- /src/thirdparty/RoboClaw/RoboClaw.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "RoboClaw.h" 3 | 4 | #define MAXRETRY 2 5 | #define SetDWORDval(arg) (uint8_t)(((uint32_t)arg)>>24),(uint8_t)(((uint32_t)arg)>>16),(uint8_t)(((uint32_t)arg)>>8),(uint8_t)arg 6 | #define SetWORDval(arg) (uint8_t)(((uint16_t)arg)>>8),(uint8_t)arg 7 | 8 | // 9 | // Constructor 10 | // 11 | RoboClaw::RoboClaw(HardwareSerial *serial, uint32_t tout) 12 | { 13 | timeout = tout; 14 | hserial = serial; 15 | #ifdef __AVR__ 16 | sserial = 0; 17 | #endif 18 | } 19 | 20 | #ifdef __AVR__ 21 | RoboClaw::RoboClaw(SoftwareSerial *serial, uint32_t tout) 22 | { 23 | timeout = tout; 24 | sserial = serial; 25 | hserial = 0; 26 | } 27 | #endif 28 | 29 | // 30 | // Destructor 31 | // 32 | RoboClaw::~RoboClaw() 33 | { 34 | } 35 | 36 | void RoboClaw::begin(long speed) 37 | { 38 | if(hserial){ 39 | hserial->begin(speed); 40 | } 41 | #ifdef __AVR__ 42 | else{ 43 | sserial->begin(speed); 44 | } 45 | #endif 46 | } 47 | 48 | bool RoboClaw::listen() 49 | { 50 | #ifdef __AVR__ 51 | if(sserial){ 52 | return sserial->listen(); 53 | } 54 | #endif 55 | return false; 56 | } 57 | 58 | bool RoboClaw::isListening() 59 | { 60 | #ifdef __AVR__ 61 | if(sserial) 62 | return sserial->isListening(); 63 | #endif 64 | return false; 65 | } 66 | 67 | bool RoboClaw::overflow() 68 | { 69 | #ifdef __AVR__ 70 | if(sserial) 71 | return sserial->overflow(); 72 | #endif 73 | return false; 74 | } 75 | 76 | int RoboClaw::peek() 77 | { 78 | if(hserial) 79 | return hserial->peek(); 80 | #ifdef __AVR__ 81 | else 82 | return sserial->peek(); 83 | #endif 84 | } 85 | 86 | size_t RoboClaw::write(uint8_t byte) 87 | { 88 | if(hserial) 89 | return hserial->write(byte); 90 | #ifdef __AVR__ 91 | else 92 | return sserial->write(byte); 93 | #endif 94 | } 95 | 96 | int RoboClaw::read() 97 | { 98 | if(hserial) 99 | return hserial->read(); 100 | #ifdef __AVR__ 101 | else 102 | return sserial->read(); 103 | #endif 104 | } 105 | 106 | int RoboClaw::available() 107 | { 108 | if(hserial) 109 | return hserial->available(); 110 | #ifdef __AVR__ 111 | else 112 | return sserial->available(); 113 | #endif 114 | } 115 | 116 | void RoboClaw::flush() 117 | { 118 | if(hserial) 119 | hserial->flush(); 120 | } 121 | 122 | int RoboClaw::read(uint32_t timeout) 123 | { 124 | if(hserial){ 125 | uint32_t start = micros(); 126 | // Empty buffer? 127 | while(!hserial->available()){ 128 | if((micros()-start)>=timeout) 129 | return -1; 130 | } 131 | return hserial->read(); 132 | } 133 | #ifdef __AVR__ 134 | else{ 135 | if(sserial->isListening()){ 136 | uint32_t start = micros(); 137 | // Empty buffer? 138 | while(!sserial->available()){ 139 | if((micros()-start)>=timeout) 140 | return -1; 141 | } 142 | return sserial->read(); 143 | } 144 | } 145 | #endif 146 | } 147 | 148 | void RoboClaw::clear() 149 | { 150 | if(hserial){ 151 | while(hserial->available()) 152 | hserial->read(); 153 | } 154 | #ifdef __AVR__ 155 | else{ 156 | while(sserial->available()) 157 | sserial->read(); 158 | } 159 | #endif 160 | } 161 | 162 | void RoboClaw::crc_clear() 163 | { 164 | crc = 0; 165 | } 166 | 167 | void RoboClaw::crc_update (uint8_t data) 168 | { 169 | int i; 170 | crc = crc ^ ((uint16_t)data << 8); 171 | for (i=0; i<8; i++) 172 | { 173 | if (crc & 0x8000) 174 | crc = (crc << 1) ^ 0x1021; 175 | else 176 | crc <<= 1; 177 | } 178 | } 179 | 180 | uint16_t RoboClaw::crc_get() 181 | { 182 | return crc; 183 | } 184 | 185 | bool RoboClaw::write_n(uint8_t cnt, ... ) 186 | { 187 | uint8_t trys=MAXRETRY; 188 | do{ 189 | crc_clear(); 190 | //send data with crc 191 | va_list marker; 192 | va_start( marker, cnt ); /* Initialize variable arguments. */ 193 | for(uint8_t index=0;index>8); 201 | write(crc); 202 | if(read(timeout)==0xFF) 203 | return true; 204 | }while(trys--); 205 | return false; 206 | } 207 | 208 | bool RoboClaw::read_n(uint8_t cnt,uint8_t address,uint8_t cmd,...) 209 | { 210 | uint32_t value=0; 211 | uint8_t trys=MAXRETRY; 212 | int16_t data; 213 | do{ 214 | flush(); 215 | 216 | data=0; 217 | crc_clear(); 218 | write(address); 219 | crc_update(address); 220 | write(cmd); 221 | crc_update(cmd); 222 | 223 | //send data with crc 224 | va_list marker; 225 | va_start( marker, cmd ); /* Initialize variable arguments. */ 226 | for(uint8_t index=0;index>8; 736 | depth2 = value; 737 | } 738 | return valid; 739 | } 740 | 741 | bool RoboClaw::ReadPWMs(uint8_t address, int16_t &pwm1, int16_t &pwm2){ 742 | bool valid; 743 | uint32_t value = Read4(address,GETPWMS,&valid); 744 | if(valid){ 745 | pwm1 = value>>16; 746 | pwm2 = value&0xFFFF; 747 | } 748 | return valid; 749 | } 750 | 751 | bool RoboClaw::ReadCurrents(uint8_t address, int16_t ¤t1, int16_t ¤t2){ 752 | bool valid; 753 | uint32_t value = Read4(address,GETCURRENTS,&valid); 754 | if(valid){ 755 | current1 = value>>16; 756 | current2 = value&0xFFFF; 757 | } 758 | return valid; 759 | } 760 | 761 | bool RoboClaw::SpeedAccelM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t accel2, uint32_t speed2){ 762 | return write_n(18,address,MIXEDSPEED2ACCEL,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(accel2),SetDWORDval(speed2)); 763 | } 764 | 765 | bool RoboClaw::SpeedAccelDistanceM1M2_2(uint8_t address, uint32_t accel1, uint32_t speed1, uint32_t distance1, uint32_t accel2, uint32_t speed2, uint32_t distance2, uint8_t flag){ 766 | return write_n(27,address,MIXEDSPEED2ACCELDIST,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(distance1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(distance2),flag); 767 | } 768 | 769 | bool RoboClaw::DutyAccelM1(uint8_t address, uint16_t duty, uint32_t accel){ 770 | return write_n(8,address,M1DUTYACCEL,SetWORDval(duty),SetDWORDval(accel)); 771 | } 772 | 773 | bool RoboClaw::DutyAccelM2(uint8_t address, uint16_t duty, uint32_t accel){ 774 | return write_n(8,address,M2DUTYACCEL,SetWORDval(duty),SetDWORDval(accel)); 775 | } 776 | 777 | bool RoboClaw::DutyAccelM1M2(uint8_t address, uint16_t duty1, uint32_t accel1, uint16_t duty2, uint32_t accel2){ 778 | return write_n(14,address,MIXEDDUTYACCEL,SetWORDval(duty1),SetDWORDval(accel1),SetWORDval(duty2),SetDWORDval(accel2)); 779 | } 780 | 781 | bool RoboClaw::ReadM1VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps){ 782 | uint32_t Kp,Ki,Kd; 783 | bool valid = read_n(4,address,READM1PID,&Kp,&Ki,&Kd,&qpps); 784 | Kp_fp = ((float)Kp)/65536; 785 | Ki_fp = ((float)Ki)/65536; 786 | Kd_fp = ((float)Kd)/65536; 787 | return valid; 788 | } 789 | 790 | bool RoboClaw::ReadM2VelocityPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &qpps){ 791 | uint32_t Kp,Ki,Kd; 792 | bool valid = read_n(4,address,READM2PID,&Kp,&Ki,&Kd,&qpps); 793 | Kp_fp = ((float)Kp)/65536; 794 | Ki_fp = ((float)Ki)/65536; 795 | Kd_fp = ((float)Kd)/65536; 796 | return valid; 797 | } 798 | 799 | bool RoboClaw::SetMainVoltages(uint8_t address,uint16_t min,uint16_t max){ 800 | return write_n(6,address,SETMAINVOLTAGES,SetWORDval(min),SetWORDval(max)); 801 | } 802 | 803 | bool RoboClaw::SetLogicVoltages(uint8_t address,uint16_t min,uint16_t max){ 804 | return write_n(6,address,SETLOGICVOLTAGES,SetWORDval(min),SetWORDval(max)); 805 | } 806 | 807 | bool RoboClaw::ReadMinMaxMainVoltages(uint8_t address,uint16_t &min,uint16_t &max){ 808 | bool valid; 809 | uint32_t value = Read4(address,GETMINMAXMAINVOLTAGES,&valid); 810 | if(valid){ 811 | min = value>>16; 812 | max = value&0xFFFF; 813 | } 814 | return valid; 815 | } 816 | 817 | bool RoboClaw::ReadMinMaxLogicVoltages(uint8_t address,uint16_t &min,uint16_t &max){ 818 | bool valid; 819 | uint32_t value = Read4(address,GETMINMAXLOGICVOLTAGES,&valid); 820 | if(valid){ 821 | min = value>>16; 822 | max = value&0xFFFF; 823 | } 824 | return valid; 825 | } 826 | 827 | bool RoboClaw::SetM1PositionPID(uint8_t address,float kp_fp,float ki_fp,float kd_fp,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max){ 828 | uint32_t kp=kp_fp*1024; 829 | uint32_t ki=ki_fp*1024; 830 | uint32_t kd=kd_fp*1024; 831 | return write_n(30,address,SETM1POSPID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(kiMax),SetDWORDval(deadzone),SetDWORDval(min),SetDWORDval(max)); 832 | } 833 | 834 | bool RoboClaw::SetM2PositionPID(uint8_t address,float kp_fp,float ki_fp,float kd_fp,uint32_t kiMax,uint32_t deadzone,uint32_t min,uint32_t max){ 835 | uint32_t kp=kp_fp*1024; 836 | uint32_t ki=ki_fp*1024; 837 | uint32_t kd=kd_fp*1024; 838 | return write_n(30,address,SETM2POSPID,SetDWORDval(kd),SetDWORDval(kp),SetDWORDval(ki),SetDWORDval(kiMax),SetDWORDval(deadzone),SetDWORDval(min),SetDWORDval(max)); 839 | } 840 | 841 | bool RoboClaw::ReadM1PositionPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max){ 842 | uint32_t Kp,Ki,Kd; 843 | bool valid = read_n(7,address,READM1POSPID,&Kp,&Ki,&Kd,&KiMax,&DeadZone,&Min,&Max); 844 | Kp_fp = ((float)Kp)/1024; 845 | Ki_fp = ((float)Ki)/1024; 846 | Kd_fp = ((float)Kd)/1024; 847 | return valid; 848 | } 849 | 850 | bool RoboClaw::ReadM2PositionPID(uint8_t address,float &Kp_fp,float &Ki_fp,float &Kd_fp,uint32_t &KiMax,uint32_t &DeadZone,uint32_t &Min,uint32_t &Max){ 851 | uint32_t Kp,Ki,Kd; 852 | bool valid = read_n(7,address,READM2POSPID,&Kp,&Ki,&Kd,&KiMax,&DeadZone,&Min,&Max); 853 | Kp_fp = ((float)Kp)/1024; 854 | Ki_fp = ((float)Ki)/1024; 855 | Kd_fp = ((float)Kd)/1024; 856 | return valid; 857 | } 858 | 859 | bool RoboClaw::SpeedAccelDeccelPositionM1(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag){ 860 | return write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); 861 | } 862 | 863 | bool RoboClaw::SpeedAccelDeccelPositionM2(uint8_t address,uint32_t accel,uint32_t speed,uint32_t deccel,uint32_t position,uint8_t flag){ 864 | return write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag); 865 | } 866 | 867 | bool RoboClaw::SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1,uint32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2,uint32_t position2,uint8_t flag){ 868 | return write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag); 869 | } 870 | 871 | bool RoboClaw::SetM1DefaultAccel(uint8_t address, uint32_t accel){ 872 | return write_n(6,address,SETM1DEFAULTACCEL,SetDWORDval(accel)); 873 | } 874 | 875 | bool RoboClaw::SetM2DefaultAccel(uint8_t address, uint32_t accel){ 876 | return write_n(6,address,SETM2DEFAULTACCEL,SetDWORDval(accel)); 877 | } 878 | 879 | bool RoboClaw::SetPinFunctions(uint8_t address, uint8_t S3mode, uint8_t S4mode, uint8_t S5mode){ 880 | return write_n(5,address,SETPINFUNCTIONS,S3mode,S4mode,S5mode); 881 | } 882 | 883 | bool RoboClaw::GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode){ 884 | uint8_t crc; 885 | bool valid = false; 886 | uint8_t val1,val2,val3; 887 | uint8_t trys=MAXRETRY; 888 | int16_t data; 889 | do{ 890 | flush(); 891 | 892 | crc_clear(); 893 | write(address); 894 | crc_update(address); 895 | write(GETPINFUNCTIONS); 896 | crc_update(GETPINFUNCTIONS); 897 | 898 | data = read(timeout); 899 | crc_update(data); 900 | val1=data; 901 | 902 | if(data!=-1){ 903 | data = read(timeout); 904 | crc_update(data); 905 | val2=data; 906 | } 907 | 908 | if(data!=-1){ 909 | data = read(timeout); 910 | crc_update(data); 911 | val3=data; 912 | } 913 | 914 | if(data!=-1){ 915 | uint16_t ccrc; 916 | data = read(timeout); 917 | if(data!=-1){ 918 | ccrc = data << 8; 919 | data = read(timeout); 920 | if(data!=-1){ 921 | ccrc |= data; 922 | if(crc_get()==ccrc){ 923 | S3mode = val1; 924 | S4mode = val2; 925 | S5mode = val3; 926 | return true; 927 | } 928 | } 929 | } 930 | } 931 | }while(trys--); 932 | 933 | return false; 934 | } 935 | 936 | bool RoboClaw::SetDeadBand(uint8_t address, uint8_t Min, uint8_t Max){ 937 | return write_n(4,address,SETDEADBAND,Min,Max); 938 | } 939 | 940 | bool RoboClaw::GetDeadBand(uint8_t address, uint8_t &Min, uint8_t &Max){ 941 | bool valid; 942 | uint16_t value = Read2(address,GETDEADBAND,&valid); 943 | if(valid){ 944 | Min = value>>8; 945 | Max = value; 946 | } 947 | return valid; 948 | } 949 | 950 | bool RoboClaw::ReadEncoders(uint8_t address,uint32_t &enc1,uint32_t &enc2){ 951 | bool valid = read_n(2,address,GETENCODERS,&enc1,&enc2); 952 | return valid; 953 | } 954 | 955 | bool RoboClaw::ReadISpeeds(uint8_t address,uint32_t &ispeed1,uint32_t &ispeed2){ 956 | bool valid = read_n(2,address,GETISPEEDS,&ispeed1,&ispeed2); 957 | return valid; 958 | } 959 | 960 | bool RoboClaw::RestoreDefaults(uint8_t address){ 961 | return write_n(2,address,RESTOREDEFAULTS); 962 | } 963 | 964 | bool RoboClaw::ReadTemp(uint8_t address, uint16_t &temp){ 965 | bool valid; 966 | temp = Read2(address,GETTEMP,&valid); 967 | return valid; 968 | } 969 | 970 | bool RoboClaw::ReadTemp2(uint8_t address, uint16_t &temp){ 971 | bool valid; 972 | temp = Read2(address,GETTEMP2,&valid); 973 | return valid; 974 | } 975 | 976 | uint32_t RoboClaw::ReadError(uint8_t address,bool *valid){ 977 | return Read4(address,GETERROR,valid); 978 | } 979 | 980 | bool RoboClaw::ReadEncoderModes(uint8_t address, uint8_t &M1mode, uint8_t &M2mode){ 981 | bool valid; 982 | uint16_t value = Read2(address,GETENCODERMODE,&valid); 983 | if(valid){ 984 | M1mode = value>>8; 985 | M2mode = value; 986 | } 987 | return valid; 988 | } 989 | 990 | bool RoboClaw::SetM1EncoderMode(uint8_t address,uint8_t mode){ 991 | return write_n(3,address,SETM1ENCODERMODE,mode); 992 | } 993 | 994 | bool RoboClaw::SetM2EncoderMode(uint8_t address,uint8_t mode){ 995 | return write_n(3,address,SETM2ENCODERMODE,mode); 996 | } 997 | 998 | bool RoboClaw::WriteNVM(uint8_t address){ 999 | return write_n(6,address,WRITENVM, SetDWORDval(0xE22EAB7A) ); 1000 | } 1001 | 1002 | bool RoboClaw::ReadNVM(uint8_t address){ 1003 | return write_n(2,address,READNVM); 1004 | } 1005 | 1006 | bool RoboClaw::SetConfig(uint8_t address, uint16_t config){ 1007 | return write_n(4,address,SETCONFIG,SetWORDval(config)); 1008 | } 1009 | 1010 | bool RoboClaw::GetConfig(uint8_t address, uint16_t &config){ 1011 | bool valid; 1012 | uint16_t value = Read2(address,GETCONFIG,&valid); 1013 | if(valid){ 1014 | config = value; 1015 | } 1016 | return valid; 1017 | } 1018 | 1019 | bool RoboClaw::SetM1MaxCurrent(uint8_t address,uint32_t max){ 1020 | return write_n(10,address,SETM1MAXCURRENT,SetDWORDval(max),SetDWORDval(0)); 1021 | } 1022 | 1023 | bool RoboClaw::SetM2MaxCurrent(uint8_t address,uint32_t max){ 1024 | return write_n(10,address,SETM2MAXCURRENT,SetDWORDval(max),SetDWORDval(0)); 1025 | } 1026 | 1027 | bool RoboClaw::ReadM1MaxCurrent(uint8_t address,uint32_t &max){ 1028 | uint32_t tmax,dummy; 1029 | bool valid = read_n(2,address,GETM1MAXCURRENT,&tmax,&dummy); 1030 | if(valid) 1031 | max = tmax; 1032 | return valid; 1033 | } 1034 | 1035 | bool RoboClaw::ReadM2MaxCurrent(uint8_t address,uint32_t &max){ 1036 | uint32_t tmax,dummy; 1037 | bool valid = read_n(2,address,GETM2MAXCURRENT,&tmax,&dummy); 1038 | if(valid) 1039 | max = tmax; 1040 | return valid; 1041 | } 1042 | 1043 | bool RoboClaw::SetPWMMode(uint8_t address, uint8_t mode){ 1044 | return write_n(3,address,SETPWMMODE,mode); 1045 | } 1046 | 1047 | bool RoboClaw::GetPWMMode(uint8_t address, uint8_t &mode){ 1048 | bool valid; 1049 | uint8_t value = Read1(address,GETPWMMODE,&valid); 1050 | if(valid){ 1051 | mode = value; 1052 | } 1053 | return valid; 1054 | } 1055 | --------------------------------------------------------------------------------