diff --git a/src/main.rs b/src/main.rs
index f976fca..e171dbc 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -47,6 +47,9 @@ pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;
 /// if your board has a different frequency
 const XTAL_FREQ_HZ: u32 = 12_000_000u32;
 
+const SWITCH_PWM_MAX: i32 = 512;
+const SWITCH_PWM_LIMIT: i32 = 93 * SWITCH_PWM_MAX / 100;
+
 fn convert_adc_measurements(raw: &[u16; 4]) -> (u32, u32, u32)
 {
     let iout = (raw[0] - raw[1]) as u32 * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt
@@ -119,14 +122,25 @@ fn main() -> ! {
     // Init PWMs
     let mut pwm_slices = hal::pwm::Slices::new(pac.PWM, &mut pac.RESETS);
 
-    // Configure PWM4
+    // Configure switch PWM
     let pwm5 = &mut pwm_slices.pwm5;
     pwm5.set_ph_correct();
     pwm5.set_div_int(4);
     pwm5.set_div_frac((88 << 4) / 100);
-    pwm5.set_top(512);
+    pwm5.set_top(SWITCH_PWM_MAX as u16);
     pwm5.enable();
 
+    // set up switch PWM output
+    // note:
+    // duty cycle = 0 => output voltage = input voltage
+    // duty cycle = 512 (100%) => input short circuit
+    //
+    // general: D = duty cycle (0..1)
+    // => output voltage = 1/(1-D) * input voltage
+    let pwr_switch_ch = &mut pwm5.channel_a;
+    pwr_switch_ch.set_duty(0); // no output by default
+    pwr_switch_ch.output_to(pins.gpio10);
+
     // LED pins
     let mut pin_led_r = pins.gpio13.into_push_pull_output();
     let mut pin_led_y = pins.gpio14.into_push_pull_output();
@@ -136,11 +150,6 @@ fn main() -> ! {
     pin_led_y.set_high().unwrap();
     pin_led_g.set_high().unwrap();
 
-    // set up switch PWM
-    let pwr_switch_ch = &mut pwm5.channel_a;
-    pwr_switch_ch.output_to(pins.gpio10);
-    pwr_switch_ch.set_duty(400);
-
     // SPI CS pin is controlled by software
     let mut spi_cs = pins.gpio5.into_push_pull_output();
     spi_cs.set_high().unwrap();
@@ -164,7 +173,19 @@ fn main() -> ! {
 
     uart.write_full_blocking(b"Initialization complete!\r\n");
 
-    // Infinite loop, fading LED up and down
+    let vtarget: i32 =  8000;
+
+    const GAINSCALE: i32 = 10000;
+    let pgain: i32 =   50;
+    let igain: i32 =  100;
+    let dgain: i32 =    0;
+
+    let mut iaccu: i32 = 0;
+    let mut elast: i32 = 0;
+
+    const T: i32 = 1;
+
+    // main loop
     loop {
         let mut adc_value: [u16; 4] = [0; 4];
 
@@ -175,6 +196,39 @@ fn main() -> ! {
 
         let (vin, vout, iout) = convert_adc_measurements(&adc_value);
 
+        // PID controller
+        let err: i32 = -(vout as i32 - vtarget);
+
+        iaccu += igain * T * err;
+
+        // limit iaccu
+        if iaccu > SWITCH_PWM_LIMIT * GAINSCALE {
+            iaccu = SWITCH_PWM_LIMIT * GAINSCALE;
+        } else if iaccu < 0 {
+            iaccu = 0;
+        }
+
+        let pval = pgain * err / GAINSCALE;
+        let ival = iaccu / GAINSCALE;
+        let dval = dgain * (err - elast) / GAINSCALE;
+
+        elast = err;
+
+        let ctrlout = pval + ival + dval;
+
+        // limit the control value
+        let pwmval;
+        if ctrlout > SWITCH_PWM_LIMIT {
+            pwmval = SWITCH_PWM_LIMIT;
+        } else if ctrlout < 0 {
+            pwmval = 0;
+        } else {
+            pwmval = ctrlout;
+        }
+
+        pwr_switch_ch.set_duty(pwmval as u16);
+
+
         {
             let mut data: String<16>;
 
@@ -199,7 +253,23 @@ fn main() -> ! {
                 }
             }
 
-            uart.write_full_blocking(b"]\r\n");
+            uart.write_full_blocking(b"]; e=");
+            data = String::from(err);
+            uart.write_full_blocking(data.as_bytes());
+            uart.write_full_blocking(b" => ");
+            data = String::from(pval);
+            uart.write_full_blocking(data.as_bytes());
+            uart.write_full_blocking(b"+");
+            data = String::from(ival);
+            uart.write_full_blocking(data.as_bytes());
+            uart.write_full_blocking(b"+");
+            data = String::from(dval);
+            uart.write_full_blocking(data.as_bytes());
+            uart.write_full_blocking(b" = ");
+            data = String::from(ctrlout);
+            uart.write_full_blocking(data.as_bytes());
+
+            uart.write_full_blocking(b"\r\n");
         }
     }
 }