Added firmware v3

firmware/pd-psu_v2/display_driver.ino

@@ -38,7 +38,7 @@ uint8_t current_dec_0;
 uint16_t* display_addr_list[16];
 
 void I2CWriteCommand(uint8_t command) {
-  I2CStart();
+  
   I2CSend(OLED_I2C_ADDRESS << 1);  // I2C address + Write bit
   I2CSend(0x00);                   // Co = 0, D/C# = 0
   I2CSend(command);

firmware/pd-psu_v3/display_char.h

@@ -0,0 +1,518 @@
+#ifndef DISPLAY_CHAR_H
+#define DISPLAY_CHAR_H
+#define display16X16_number { \
+    display16X16_0, \
+    display16X16_1, \
+    display16X16_2, \
+    display16X16_3, \
+    display16X16_4, \
+    display16X16_5, \
+    display16X16_6, \
+    display16X16_7, \
+    display16X16_8, \
+    display16X16_9  \
+}
+#define display16X16_0  { \
+    0b0000000000000000, \
+    0b0000011111100000, \
+    0b0000111111110000, \
+    0b0001100000011000, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0001100000011000, \
+    0b0000111111110000, \
+    0b0000011111100000, \
+    0b0000000000000000  \
+}
+#define display16X16_1  { \
+    0b0000000000000000, \
+    0b0000000111000000, \
+    0b0000001111000000, \
+    0b0000011111000000, \
+    0b0000110111000000, \
+    0b0001100111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0000000111000000, \
+    0b0011111111111100, \
+    0b0011111111111100, \
+    0b0000000000000000  \
+}
+
+#define display16X16_2  { \
+    0b0000000000000000, \
+    0b0000011111110000, \
+    0b0000111111111000, \
+    0b0001110000011100, \
+    0b0011000000001100, \
+    0b0011000000011100, \
+    0b0000000000111000, \
+    0b0000000001110000, \
+    0b0000000011100000, \
+    0b0000000111000000, \
+    0b0000001110000000, \
+    0b0000011100000000, \
+    0b0000111000000000, \
+    0b0011111111111100, \
+    0b0011111111111100, \
+    0b0000000000000000  \
+}
+
+#define display16X16_3  { \
+    0b0000000000000000, \
+    0b0000011111110000, \
+    0b0000111111111000, \
+    0b0001110000011100, \
+    0b0011000000001100, \
+    0b0000000000011000, \
+    0b0000000001110000, \
+    0b0000000111110000, \
+    0b0000000111110000, \
+    0b0000000000011000, \
+    0b0000000000001100, \
+    0b0011000000001100, \
+    0b0001110000011100, \
+    0b0000111111111000, \
+    0b0000011111110000, \
+    0b0000000000000000  \
+}
+
+#define display16X16_4  { \
+    0b0000000000000000, \
+    0b0000000001110000, \
+    0b0000000011110000, \
+    0b0000000111110000, \
+    0b0000001111110000, \
+    0b0000011101110000, \
+    0b0000111001110000, \
+    0b0001110001110000, \
+    0b0011100001110000, \
+    0b0111000001110000, \
+    0b1111111111111100, \
+    0b1111111111111100, \
+    0b0000000001110000, \
+    0b0000000001110000, \
+    0b0000000001110000, \
+    0b0000000000000000  \
+}
+
+#define display16X16_5  { \
+    0b0000000000000000, \
+    0b0011111111111100, \
+    0b0011111111111100, \
+    0b0011000000000000, \
+    0b0011000000000000, \
+    0b0011000000000000, \
+    0b0011111111110000, \
+    0b0011111111111000, \
+    0b0011000000011100, \
+    0b0000000000001100, \
+    0b0000000000001100, \
+    0b0011000000011100, \
+    0b0011000000011000, \
+    0b0001111111111000, \
+    0b0000111111110000, \
+    0b0000000000000000  \
+}
+#define display16X16_6  { \
+    0b0000000000000000, \
+    0b0000001111110000, \
+    0b0000011111111000, \
+    0b0000111000011100, \
+    0b0001110000001100, \
+    0b0001100000000000, \
+    0b0011000000000000, \
+    0b0011001111110000, \
+    0b0011111000011100, \
+    0b0011100000001100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0011100000011100, \
+    0b0001111111111000, \
+    0b0000111111110000, \
+    0b0000000000000000  \
+}
+#define display16X16_7  { \
+    0b0000000000000000, \
+    0b0011111111111100, \
+    0b0011111111111100, \
+    0b0000000000011000, \
+    0b0000000000110000, \
+    0b0000000001100000, \
+    0b0000000011000000, \
+    0b0000000110000000, \
+    0b0000000110000000, \
+    0b0000001100000000, \
+    0b0000011000000000, \
+    0b0000011000000000, \
+    0b0000110000000000, \
+    0b0000110000000000, \
+    0b0000110000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_8  { \
+    0b0000000000000000, \
+    0b0000011111110000, \
+    0b0000111111111000, \
+    0b0001110000011100, \
+    0b0001100000001100, \
+    0b0001100000001100, \
+    0b0000110000111000, \
+    0b0000011111110000, \
+    0b0000111111111000, \
+    0b0001100000011100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0001100000011100, \
+    0b0001111111111000, \
+    0b0000111111110000, \
+    0b0000000000000000  \
+}
+#define display16X16_9  { \
+    0b0000000000000000, \
+    0b0000011111110000, \
+    0b0000111111111000, \
+    0b0001110000011100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0011000000011100, \
+    0b0001111111111000, \
+    0b0000111111111000, \
+    0b0000000000011000, \
+    0b0000000000110000, \
+    0b0000000000110000, \
+    0b0000000001100000, \
+    0b0001111111000000, \
+    0b0001111110000000, \
+    0b0000000000000000  \
+}
+#define display16X16_A  { \
+    0b0000000000000000, \
+    0b0000001111000000, \
+    0b0000011111100000, \
+    0b0000111001110000, \
+    0b0000110000110000, \
+    0b0001100000011000, \
+    0b0001100000011000, \
+    0b0001111111111000, \
+    0b0011111111111100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0111000000001110, \
+    0b0111000000001110, \
+    0b0000000000000000  \
+}
+#define display16X16_C  { \
+    0b0000000000000000, \
+    0b0000001111110000, \
+    0b0000011111111000, \
+    0b0000111000011100, \
+    0b0001110000001100, \
+    0b0011100000000000, \
+    0b0011000000000000, \
+    0b0111000000000000, \
+    0b0111000000000000, \
+    0b0111000000000000, \
+    0b0011000000000000, \
+    0b0011100000000000, \
+    0b0001110000001100, \
+    0b0000111000011100, \
+    0b0000011111111000, \
+    0b0000001111110000  \
+}
+#define display16X16_V  { \
+    0b0000000000000000, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0111000000001110, \
+    0b0011000000001100, \
+    0b0011000000001100, \
+    0b0011100000011100, \
+    0b0001100000011000, \
+    0b0001100000011000, \
+    0b0000110000110000, \
+    0b0000110000110000, \
+    0b0000111001110000, \
+    0b0000011011100000, \
+    0b0000011111100000, \
+    0b0000001111000000, \
+    0b0000000000000000  \
+}
+#define display16X16_blank { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_number_R { \
+    display16X16_0_R, \
+    display16X16_1_R, \
+    display16X16_2_R, \
+    display16X16_3_R, \
+    display16X16_4_R, \
+    display16X16_5_R, \
+    display16X16_6_R, \
+    display16X16_7_R, \
+    display16X16_8_R, \
+    display16X16_9_R  \
+}
+#define display16X16_0_R  { \
+    0b0000000000000000, \
+    0b0000001111000000, \
+    0b0000111111110000, \
+    0b0001110000111000, \
+    0b0011000000001100, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0110000000000110, \
+    0b0011000000001100, \
+    0b0001110000111000, \
+    0b0000111111110000, \
+    0b0000001111000000, \
+    0b0000000000000000  \
+}
+
+#define display16X16_1_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0110000000000000, \
+    0b0110000000100000, \
+    0b0110000000110000, \
+    0b0110000000011000, \
+    0b0110000000001100, \
+    0b0111111111111110, \
+    0b0111111111111110, \
+    0b0111111111111110, \
+    0b0110000000000000, \
+    0b0110000000000000, \
+    0b0110000000000000, \
+    0b0110000000000000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_2_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0110000000110000, \
+    0b0110000000111000, \
+    0b0111000000001100, \
+    0b0111100000001110, \
+    0b0111110000000110, \
+    0b0110111000000110, \
+    0b0110011100000110, \
+    0b0110001110000110, \
+    0b0110000111000110, \
+    0b0110000011101110, \
+    0b0110000001111100, \
+    0b0110000000111000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_3_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000100000010000, \
+    0b0001100000011000, \
+    0b0011000000001100, \
+    0b0111000000001110, \
+    0b0110000000000110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0110000111000110, \
+    0b0110000111000110, \
+    0b0111001111101110, \
+    0b0011111000111100, \
+    0b0001110000011000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_4_R  { \
+    0b0000110000000000, \
+    0b0000111000000000, \
+    0b0000111100000000, \
+    0b0000111110000000, \
+    0b0000110111000000, \
+    0b0000110011100000, \
+    0b0000110001110000, \
+    0b0000110000111000, \
+    0b0000110000011100, \
+    0b0111111111111110, \
+    0b0111111111111110, \
+    0b0111111111111110, \
+    0b0000110000000000, \
+    0b0000110000000000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_5_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0001100111111110, \
+    0b0011100111111110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0110000011000110, \
+    0b0111100111000110, \
+    0b0011111110000110, \
+    0b0000111100000110, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_6_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0001111111000000, \
+    0b0011111111110000, \
+    0b0111001100111000, \
+    0b0110000100011100, \
+    0b0110000110001110, \
+    0b0110000010000110, \
+    0b0110000010000110, \
+    0b0110000010000110, \
+    0b0110000010000110, \
+    0b0111000110001110, \
+    0b0011111100011100, \
+    0b0001111100011000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_7_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000000000110, \
+    0b0000000000000110, \
+    0b0111000000000110, \
+    0b0111110000000110, \
+    0b0000111000000110, \
+    0b0000001110000110, \
+    0b0000000111000110, \
+    0b0000000001100110, \
+    0b0000000000110110, \
+    0b0000000000011110, \
+    0b0000000000001110, \
+    0b0000000000000110, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_8_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000110000000000, \
+    0b0011111000111000, \
+    0b0111001101111100, \
+    0b0110000111001110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0110000111000110, \
+    0b0111001111001110, \
+    0b0011111101111100, \
+    0b0001111000111000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_9_R  { \
+    0b0000000000000000, \
+    0b0000000000000000, \
+    0b0000000001110000, \
+    0b0110000011111000, \
+    0b0110000110001100, \
+    0b0110000110001110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0110000110000110, \
+    0b0011000110000110, \
+    0b0001110110000110, \
+    0b0000111111001110, \
+    0b0000001111111100, \
+    0b0000000001111000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_A_R  { \
+    0b0000000000000000, \
+    0b0110000000000000, \
+    0b0111111100000000, \
+    0b0111111111100000, \
+    0b0000000111111000, \
+    0b0000000110011100, \
+    0b0000000110001110, \
+    0b0000000110000110, \
+    0b0000000110000110, \
+    0b0000000110001110, \
+    0b0000000110011100, \
+    0b0000000111111000, \
+    0b0111111111100000, \
+    0b0111111100000000, \
+    0b0110000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_C_R  { \
+    0b0000000000000000, \
+    0b0000001110000000, \
+    0b0000111111100000, \
+    0b0001111111110000, \
+    0b0011100000111000, \
+    0b0111000000011100, \
+    0b1110000000001110, \
+    0b1100000000000110, \
+    0b1100000000000110, \
+    0b1100000000000110, \
+    0b1100000000000110, \
+    0b1110000000001110, \
+    0b0111000000011100, \
+    0b0011000000011000, \
+    0b0000000000000000, \
+    0b0000000000000000  \
+}
+#define display16X16_V_R  { \
+    0b0000000000000000, \
+    0b0000000000001110, \
+    0b0000000001111110, \
+    0b0000000111111000, \
+    0b0000111111000000, \
+    0b0011111000000000, \
+    0b0111100000000000, \
+    0b0110000000000000, \
+    0b0111000000000000, \
+    0b0111100000000000, \
+    0b0011111000000000, \
+    0b0000111111000000, \
+    0b0000000111111000, \
+    0b0000000001111110, \
+    0b0000000000001110, \
+    0b0000000000000000  \
+}
+#endif

firmware/pd-psu_v3/display_driver.ino

@@ -0,0 +1,179 @@
+#include <SoftI2C.h>
+#include "display_char.h"
+
+// Address of the I2C OLED
+#define OLED_I2C_ADDRESS 0x3C
+
+// Commands for SSD1306
+#define SSD1306_DISPLAYOFF 0xAE
+#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
+#define SSD1306_SETMULTIPLEX 0xA8
+#define SSD1306_SETDISPLAYOFFSET 0xD3
+#define SSD1306_SETSTARTLINE 0x40
+#define SSD1306_CHARGEPUMP 0x8D
+#define SSD1306_MEMORYMODE 0x20
+#define SSD1306_SEGREMAP 0xA1
+#define SSD1306_COMSCANDEC 0xC8
+#define SSD1306_SETCOMPINS 0xDA
+#define SSD1306_SETCONTRAST 0x81
+#define SSD1306_SETPRECHARGE 0xD9
+#define SSD1306_SETVCOMDETECT 0xDB
+#define SSD1306_DISPLAYALLON_RESUME 0xA4
+#define SSD1306_NORMALDISPLAY 0xA6
+#define SSD1306_DISPLAYON 0xAF
+
+//Global Variables
+static uint16_t display_num[10][16] = display16X16_number_R;
+static uint16_t display_A[16] = display16X16_A_R;
+static uint16_t display_C[16] = display16X16_C_R;
+static uint16_t display_V[16] = display16X16_V_R;
+static uint16_t display_blank[16] = display16X16_blank;
+uint8_t voltage_dec_3;
+uint8_t voltage_dec_2;
+uint8_t voltage_dec_1;
+uint8_t voltage_dec_0;
+uint8_t current_dec_2;
+uint8_t current_dec_1;
+uint8_t current_dec_0;
+uint16_t* display_addr_list[16];
+
+void I2CWriteCommand(uint8_t command) {
+  I2CStart();
+  I2CSend(OLED_I2C_ADDRESS << 1);  // I2C address + Write bit
+  I2CSend(0x00);                   // Co = 0, D/C# = 0
+  I2CSend(command);
+  I2CStop();
+}
+
+void I2CWriteData(uint8_t data) {
+  I2CStart();
+  I2CSend(OLED_I2C_ADDRESS << 1);  // I2C address + Write bit
+  I2CSend(0x40);                   // Co = 0, D/C# = 1
+  I2CSend(data);
+  I2CStop();
+}
+
+//Setup the OLED SoftI2C Config, call to initializeOLED to start connection
+void setupOLED() {
+  scl_pin = OLED_SCL;  // Default: P3.0
+  sda_pin = OLED_SDA;  // Default: P3.1
+  I2CInit();
+
+  //Setup static locations for display array
+  display_addr_list[2] = display_blank;
+  display_addr_list[7] = display_V;
+  display_addr_list[10] = display_blank;
+  display_addr_list[11] = display_blank;
+  display_addr_list[15] = display_A;
+}
+
+//Start the OLED screen in desired mode
+void initializeOLED() {
+  I2CWriteCommand(SSD1306_DISPLAYOFF);
+  I2CWriteCommand(SSD1306_SETDISPLAYCLOCKDIV);
+  I2CWriteCommand(0x80);  // Suggested value
+  I2CWriteCommand(SSD1306_SETMULTIPLEX);
+  I2CWriteCommand(0x3F);
+  I2CWriteCommand(SSD1306_SETDISPLAYOFFSET);
+  I2CWriteCommand(0x00);
+  I2CWriteCommand(SSD1306_SETSTARTLINE | 0x0);
+  I2CWriteCommand(SSD1306_CHARGEPUMP);
+  I2CWriteCommand(0x14);
+  I2CWriteCommand(SSD1306_MEMORYMODE);
+  I2CWriteCommand(0x00);
+  I2CWriteCommand(SSD1306_SEGREMAP | 0x1);
+  I2CWriteCommand(SSD1306_COMSCANDEC);
+  I2CWriteCommand(SSD1306_SETCOMPINS);
+  I2CWriteCommand(0x12);
+  I2CWriteCommand(SSD1306_SETCONTRAST);
+  I2CWriteCommand(0xCF);
+  I2CWriteCommand(SSD1306_SETPRECHARGE);
+  I2CWriteCommand(0xF1);
+  I2CWriteCommand(SSD1306_SETVCOMDETECT);
+  I2CWriteCommand(0x40);
+  I2CWriteCommand(SSD1306_DISPLAYALLON_RESUME);
+  I2CWriteCommand(SSD1306_NORMALDISPLAY);
+  I2CWriteCommand(SSD1306_DISPLAYON);
+}
+
+//Clear the screen with setting all bits to 0
+void clearScreen() {
+  for (uint8_t i = 0; i < 8; i++) {
+    I2CWriteCommand(0xB0 + i);  // Set page address
+    I2CWriteCommand(0x00);      // Set lower column address
+    I2CWriteCommand(0x10);      // Set higher column address
+    for (uint8_t j = 0; j < 128; j++) {
+      I2CWriteData(0x00);
+    }
+  }
+}
+
+//Clear the screen with setting all bits to 1
+void lightScreen() {
+  for (uint8_t i = 0; i < 8; i++) {
+    I2CWriteCommand(0xB0 + i);  // Set page address
+    I2CWriteCommand(0x00);      // Set lower column address
+    I2CWriteCommand(0x10);      // Set higher column address
+    for (uint8_t j = 0; j < 128; j++) {
+      I2CWriteData(0xFF);
+    }
+  }
+}
+
+//Print the current power information to the screen
+void PS_Screen(uint16_t voltage_num, uint16_t current_num, bool CC_mode) {
+  voltage_dec_3 = (voltage_num / 1000) % 10;
+  voltage_dec_2 = (voltage_num / 100) % 10;
+  voltage_dec_1 = (voltage_num / 10) % 10;
+  voltage_dec_0 = voltage_num % 10;
+  current_dec_2 = (current_num / 100) % 10;
+  current_dec_1 = (current_num / 10) % 10;
+  current_dec_0 = current_num % 10;
+  if (!CC_mode) {
+    display_addr_list[0] = display_C;
+    display_addr_list[1] = display_V;
+    display_addr_list[8] = display_blank;
+    display_addr_list[9] = display_blank;
+  } else {
+    display_addr_list[0] = display_blank;
+    display_addr_list[1] = display_blank;
+    display_addr_list[8] = display_C;
+    display_addr_list[9] = display_C;
+  }
+  display_addr_list[3] = display_num[voltage_dec_3];
+  display_addr_list[4] = display_num[voltage_dec_2];
+  display_addr_list[5] = display_num[voltage_dec_1];
+  display_addr_list[6] = display_num[voltage_dec_0];
+  display_addr_list[12] = display_num[current_dec_2];
+  display_addr_list[13] = display_num[current_dec_1];
+  display_addr_list[14] = display_num[current_dec_0];
+  
+  uint8_t push_temp;
+  uint8_t readed_byte;
+  for (uint8_t i = 0; i < 8; i++) {
+    //Set editing page number
+    I2CStart();
+    I2CSend(OLED_I2C_ADDRESS << 1);  // I2C address + Write bit
+    I2CSend(0xB0 + i);               // Set page address
+    //I2CSend(0x00);      // Set lower column address
+    //I2CSend(0x10);      // Set higher column address
+    //Start sending page data
+    I2CRestart();
+    I2CSend(OLED_I2C_ADDRESS << 1);  // I2C address + Write bit
+    I2CSend(0x40);                   // Co = 0, D/C# = 1
+    for (uint8_t j = 0; j < 128; j++) {
+      if (i % 4 == 3 && (j == 79 || j == 80)) {
+        I2CSend(0xA0);
+      } else {
+        push_temp = 0;
+        readed_byte = display_addr_list[(j >> 4) + (i / 4 * 8)][j % 16] >> (i % 4 << 2);
+        push_temp |= ((readed_byte >> 3) & 0x01) << 7;
+        push_temp |= ((readed_byte >> 2) & 0x01) << 5;
+        push_temp |= ((readed_byte >> 1) & 0x01) << 3;
+        push_temp |= (readed_byte & 0x01) << 1;
+        I2CSend(push_temp);
+      }
+    }
+    I2CStop();
+  }
+}

firmware/pd-psu_v3/pd-psu_v3.ino

@@ -0,0 +1,69 @@
+/*
+    PD Power Supply
+    Author: 
+
+    This is the firmware for the PD power supply 
+    that reads the voltage and current of the supply and display the 
+    power information on the SSD1306 OLED display.
+
+    This firmware is designed for CH552G with CH55xduino board definations
+    Recommend config
+    24Mhz (Internal), 5V
+
+*/
+#include <Serial.h>
+
+/* Hardware Definations */
+#define CURRENT_PIN 14  //0 - 5A
+#define VOLTAGE_PIN 32  //0 - 20V
+#define CCCV_PIN 15     //CC = LOW, CV = HIGH
+#define OLED_SCL 30
+#define OLED_SDA 31
+
+/* Global Variables */
+uint16_t voltageReading;
+uint16_t currentReading;
+bool isCCMode;
+
+/* Function Prototypes */
+void setupOLED();
+void initializeOLED();
+void clearScreen();
+void PS_Screen(uint16_t, uint16_t, bool);
+
+void setup() {
+  //Disable internal pull-up resistors for input pins
+  pinMode(CURRENT_PIN, INPUT);
+  pinMode(VOLTAGE_PIN, INPUT);
+  pinMode(CCCV_PIN, INPUT);
+
+  // Initialize I2C pins for OLED
+  setupOLED();
+  initializeOLED();
+
+  //Clear the OLED screen
+  clearScreen();
+}
+
+void loop() {
+  //Read the analog reading of both pins
+  voltageReading = analogRead(VOLTAGE_PIN);
+  currentReading = analogRead(CURRENT_PIN);
+  isCCMode = !digitalRead(CCCV_PIN);
+
+  //Convert them to actual voltage / current value
+  voltageReading = map(voltageReading, 0, 255, 0, 500) * 4;  //30k - 10k voltage divider
+  currentReading = (2.5 - (currentReading * (5.0 / 255)) )/0.185;      //Max range 0 to 5A
+
+  //Cap the range of digits to make sure it won't overflow
+  voltageReading = min(voltageReading, 9999);
+  currentReading = min(currentReading, 999);
+
+  //Print result on USB Serial
+  USBSerial_print(((float)voltageReading/100));
+  USBSerial_print(",");
+  USBSerial_println(((float)currentReading/100));
+  
+  //Update OLED display
+  PS_Screen(voltageReading, currentReading, isCCMode);
+}