diff --git a/src/key_operations.rs b/src/key_operations.rs
index 8cbc32b..85e0816 100644
--- a/src/key_operations.rs
+++ b/src/key_operations.rs
@@ -290,13 +290,24 @@ enum MouseButton {
     MouseDouble = 8,
 }
 
-pub fn encode_byte(c: &str) -> u8 {
+pub fn valid_key(c: &str) -> bool {
     for key in KEY_MAP.iter() {
         if key.0 == c {
-            return key.1
+            return true;
         }
     }
-    return 0
+
+    false
+}
+
+pub fn encode_byte(c: &str) -> Option<u8> {
+    for key in KEY_MAP.iter() {
+        if key.0 == c {
+            return Some(key.1)
+        }
+    }
+
+    None
 }
 
 pub fn decode_byte(u: u8) -> Option<String> {
@@ -305,7 +316,8 @@ pub fn decode_byte(u: u8) -> Option<String> {
             return Some(key.0.to_string())
         }
     }
-    return None;
+
+    None
 }
 
 pub fn print_key(response: &[u8]) -> Option<String> {
@@ -319,7 +331,7 @@ pub fn print_key(response: &[u8]) -> Option<String> {
         }
     }
     
-    return None;
+    None
 }
 
 pub fn print_key_map(rows: usize) {
diff --git a/src/main.rs b/src/main.rs
index ae2b105..acb0326 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -32,7 +32,7 @@ struct Opt {
 }
 
 fn main() {
-    let pedals = pedal_operations::Pedals::new(); 
+    let mut pedals = pedal_operations::Pedals::new(); 
     check_sudo();
 
     let opt = Opt::from_args();
@@ -70,15 +70,14 @@ fn main() {
     let dev = api.open_path(dev_path.as_str()).unwrap();
     println!("Succesfully opened device.");
 
+    //ToDo: This part of the code is just there to test functions
+    pedals.set_key(0, "b");
+    pedals.write_pedals(& dev);
+
     // All options that need the device to be open
     if opt.read {
         pedals.read_pedals(& dev);
     }
-
-    //ToDo: set right if condition
-    if true {
-        pedals.write_pedals(& dev);
-    }
 }
 
 /// Checks if user is super user
diff --git a/src/pedal_operations.rs b/src/pedal_operations.rs
index 1d01f9a..0ea5927 100644
--- a/src/pedal_operations.rs
+++ b/src/pedal_operations.rs
@@ -18,9 +18,9 @@ impl Pedals {
         // Prepare variables
         let start = [0x01u8, 0x80, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00];
 
-        let header_0 = [0x01u8, 0x81, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00];
-        let header_1 = [0x01u8, 0x81, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00];
-        let header_2 = [0x01u8, 0x81, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00];
+        let header_0 = [0x01u8, 0x81, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00];
+        let header_1 = [0x01u8, 0x81, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00];
+        let header_2 = [0x01u8, 0x81, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00];
 
         let mut default_data = [0u8; 48];
         default_data[0] = 0x08;
@@ -92,17 +92,23 @@ impl Pedals {
         dev.write(&self.ped_data[ped].header).unwrap();
 
         // Write data to device in 8 byte chunks
+        let mut up:usize = 0;
+
         for i in 0..(self.ped_data[ped].length / 8) {
             // Set bounds
-
-            let low:usize = (i * 8) as usize;
-            let up:usize = 8 * (i + 1) as usize;
+            let low = (i * 8) as usize;
+            up  = 8 * (i + 1) as usize;
 
             // Write to device
             dev.write(&self.ped_data[ped].data[low..up]).unwrap();
         }
-    }
 
+        // Write remaining values to device
+        if self.ped_data[ped].length % 8 > 0 {
+            dev.write(&self.ped_data[ped].data[up..(self.ped_data[ped].length as usize)]).unwrap();
+        }
+    }
+    
     /// This method writes all data from Pedals.peddata to the device
     pub fn write_pedals(&self, dev: & hidapi::HidDevice) {
         dev.write(&self.start).unwrap();
@@ -111,4 +117,16 @@ impl Pedals {
             self.write_pedal(dev, i)
         }
     }
+
+    pub fn set_key(& mut self, ped:usize, key:&str) {
+
+        if let Some(encoded_key) = key_operations::encode_byte(key) {
+            self.ped_data[ped].data[1] = 1;
+            self.ped_data[ped].data[3] = encoded_key;
+        }
+        else {
+            //ToDo: add "print list" if value is not recognized
+        }
+    }
+
 }