diff --git a/corrections.ino b/corrections.ino
index a8304419..6932220c 100644
--- a/corrections.ino
+++ b/corrections.ino
@@ -207,7 +207,7 @@ This continues until either:
 PID (Best suited to wideband sensors):
  
 */
-double PID_O2, PID_output, PID_AFRTarget;
+long PID_O2, PID_output, PID_AFRTarget;
 PID egoPID(&PID_O2, &PID_output, &PID_AFRTarget, configPage3.egoKP, configPage3.egoKI, configPage3.egoKD, REVERSE); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
 
 byte correctionsAFRClosedLoop()
@@ -255,10 +255,10 @@ byte correctionsAFRClosedLoop()
       {
         //*************************************************************************************************************************************
         //PID algorithm
-        egoPID.SetOutputLimits((double)(-configPage3.egoLimit), (double)(configPage3.egoLimit)); //Set the limits again, just incase the user has changed them since the last loop. Note that these are sent to the PID library as (Eg:) -15 and +15
+        egoPID.SetOutputLimits((long)(-configPage3.egoLimit), (long)(configPage3.egoLimit)); //Set the limits again, just incase the user has changed them since the last loop. Note that these are sent to the PID library as (Eg:) -15 and +15
         egoPID.SetTunings(configPage3.egoKP, configPage3.egoKI, configPage3.egoKD); //Set the PID values again, just incase the user has changed them since the last loop
-        PID_O2 = (double)(currentStatus.O2);
-        PID_AFRTarget = (double)(currentStatus.afrTarget);
+        PID_O2 = (long)(currentStatus.O2);
+        PID_AFRTarget = (long)(currentStatus.afrTarget);
         
         egoPID.Compute();
         //currentStatus.egoCorrection = 100 + PID_output;
diff --git a/globals.h b/globals.h
index 096bd366..1d0b187b 100644
--- a/globals.h
+++ b/globals.h
@@ -317,9 +317,11 @@ struct config3 {
   byte lnchRetard;
   byte lnchHardLim;
   byte lnchFuelAdd;
-  byte unused53;
-  byte unused54;
-  byte unused55;
+
+  //PID values for idle needed to go here as out of room in the idle page
+  byte idleKP;
+  byte idleKI;
+  byte idleKD;
   byte unused56;
   byte unused57;
   byte unused58;
diff --git a/idle.h b/idle.h
index a3823c61..f2a25527 100644
--- a/idle.h
+++ b/idle.h
@@ -1,3 +1,5 @@
+#include "globals.h"
+
 #define STEPPER_FORWARD 0
 #define STEPPER_BACKWARD 1
 #define IDLE_TABLE_SIZE 10
@@ -30,7 +32,8 @@ volatile byte idle2_pin_mask;
 volatile bool idle_pwm_state;
 unsigned int idle_pwm_max_count; //Used for variable PWM frequency
 volatile unsigned int idle_pwm_cur_value;
-unsigned int idle_pwm_target_value;
+long idle_pwm_target_value;
+long idle_cl_target_rpm;
 
 void initialiseIdle();
 
diff --git a/idle.ino b/idle.ino
index 9906b50e..c7b35ff4 100644
--- a/idle.ino
+++ b/idle.ino
@@ -12,6 +12,9 @@ These functions cover the PWM and stepper idle control
 Idle Control
 Currently limited to on/off control and open loop PWM and stepper drive
 */
+long longRPM;
+PID idlePID(&longRPM, &idle_pwm_target_value, &idle_cl_target_rpm, configPage3.idleKP, configPage3.idleKI, configPage3.idleKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
+
 void initialiseIdle()
 {
 //By default, turn off the PWM interrupt (It gets turned on below if needed)  
@@ -62,6 +65,14 @@ void initialiseIdle()
       iacCrankDutyTable.xSize = 4;
       iacCrankDutyTable.values = configPage4.iacCrankDuty;
       iacCrankDutyTable.axisX = configPage4.iacCrankBins;
+
+      idle_pin_port = portOutputRegister(digitalPinToPort(pinIdle1));
+      idle_pin_mask = digitalPinToBitMask(pinIdle1);
+      idle2_pin_port = portOutputRegister(digitalPinToPort(pinIdle2));
+      idle2_pin_mask = digitalPinToBitMask(pinIdle2);
+      idle_pwm_max_count = 1000000L / (16 * configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 512hz
+      idlePID.SetOutputLimits(0, idle_pwm_max_count);
+      TIMSK4 |= (1 << OCIE4C); //Turn on the C compare unit (ie turn on the interrupt)
       break;
       
     case 4:
@@ -133,7 +144,13 @@ void idleControl()
       else if (idleOn) { digitalWrite(pinIdle1, LOW); idleOn = false; }
       break;
       
-    case 3:    //Case 3 is PWM closed loop (Not currently implemented)
+    case 3:    //Case 3 is PWM closed loop
+        //No cranking specific value for closed loop (yet?)
+        idle_cl_target_rpm = table2D_getValue(&iacClosedLoopTable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET) * 2; //All temps are offset by 40 degrees
+        longRPM = currentStatus.RPM; //The PID object needs a long as the RPM input. A separate variable is used for this
+        //idle_pwm_target_value = percentage(currentStatus.idleDuty, idle_pwm_max_count);
+
+        idlePID.Compute();
       break;
       
     case 4:    //Case 4 is open loop stepper control