diff --git a/Firmware/MS-Main.ino b/Firmware/MS-Main.ino
new file mode 100644
index 0000000..27b4f76
--- /dev/null
+++ b/Firmware/MS-Main.ino
@@ -0,0 +1,69 @@
+unsigned int vinSlopeCounter = 0;
+unsigned int posRefSlopeCounter = 0;
+unsigned int negRefSlopeCounter = 0;
+unsigned int vinBaseSlope = 2; // * 255
+byte nextSlopeAction = 0;
+double accumulator = 0;
+
+// RES CLK  = PORTD B7
+// RST SW   = PORTD B5
+// SLOPEIN1 = PORTD B2
+// SLOPEUP1 = PORTD B1
+// SLOPEDN1 = PORTD B0
+#define BLANKEDREG     0b00000000
+#define ENABLEINT      0b00000001
+#define SLOPESAMPLE    0b10000000
+#define SLOPERST       0b00100000
+#define SLOPEDN1       0b00000001
+#define SLOPEUP1       0b00000010
+#define SLOPEIN1       0b00000100
+#define SLOPEHOLD1     0b00000011
+
+void startConv() {
+    PORTD   = SLOPERST;          //reset charge
+    PORTD   = BLANKEDREG;        //release reset
+    PORTD   = SLOPESAMPLE;       //sample residual charge
+    PORTD   = BLANKEDREG;
+    
+    EICRA   = ENABLEINT;          //enable interrupt for zerocrossing, pin D2 (INT0)
+    TCCR1A  = BLANKEDREG;
+    TCCR1B  = BLANKEDREG;
+    //DataReg = tmpControlPinBank; //read charge value
+}
+
+void stopConv() {
+    PORTD   = BLANKEDREG;        //release reset
+    PORTD   = SLOPESAMPLE;       //sample residual charge
+    PORTD   = BLANKEDREG;        //send clock to sample charge
+    //DataReg = tmpControlPinBank; //read charge value
+    EICRA   = BLANKEDREG;        //disable interrupt for zerocrossing, pin D2 (INT0)
+
+}
+
+ISR(TIMER1_COMPA_vect) { // if 16b timer1 match
+    // Decide to slope up or down, takes n clocks
+    if(PORTD & 0b1000000)              // if comparator 1 (ADC could also be used)
+        { nextSlopeAction = SLOPEDN1; }// slope down
+    else
+        { nextSlopeAction = SLOPEUP1; }// slope up
+    PORTD = SLOPEHOLD1;                // hold pattern
+    EICRA = ENABLEINT;                 //interrupt for zerocrossing, pin D2
+    PORTD = nextSlopeAction;           // set slope down or up
+    vinSlopeCounter = TCNT1H >> 8 + TCNT1L + 7;
+    }
+
+ISR(INT0_vect) {
+    EICRA = BLANKEDREG;                // no more interrupt for 0 cross
+    PORTD = SLOPEHOLD1;                // hold pattern
+    PORTD = SLOPEIN1;                  // slope up/down to Vin for n clock
+    // now it is sloping again, compensate the clocks the transition
+    OCR1AH = OCR1AH + vinBaseSlope;  // set next slope timer
+    //timerreg = timerreg - x;
+}
+
+void setup() {
+startConv();
+delay(10);
+stopConv();
+}
+void loop() {}
\ No newline at end of file