diff --git a/src/main.rs b/src/main.rs
index 0799f9b..f4ba9ed 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -33,7 +33,6 @@ struct Opt {
 
 fn main() {
     let pedals = pedal_operations::Pedals::new(); 
-    println!("{:?}", pedals.ped_data[1].length);
     check_sudo();
 
     let opt = Opt::from_args();
@@ -71,7 +70,12 @@ fn main() {
 
     // All options that need the device to be open
     if opt.read {
-        read_pedals(& dev);
+        pedals.read_pedals(& dev);
+    }
+
+    //ToDo: set right if condition
+    if true {
+        pedals.write_pedals(& dev);
     }
 }
 
@@ -81,37 +85,3 @@ fn check_sudo() {
         panic!("Please execute this application as super user!");
     }
 }
-
-/// Read the current values of the pedals
-//ToDo: Move to pedals struct and split read and interpret functionality
-fn read_pedals(dev: & hidapi::HidDevice) {
-    let column_width = 15;
-    let total_width = (column_width+3)*3;
-
-    let mut buf = [0u8; 8];
-    let mut query = [0x01u8, 0x82, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00];
-
-    // Print header
-    println!(" {}", "-".repeat(total_width));        
-    println!(" ‖{name:^width$}‖", name = "Programmed Keys", width = total_width - 2);
-    println!(" {}", "-".repeat(total_width));        
-
-    // Read and print keys
-    for i in 0..3 {
-        query[3] = i + 1;
-
-        dev.write(&query).unwrap();
-
-        let res = dev.read(&mut buf[..]).unwrap();
-
-        
-        let key_name = match key_operations::print_key(&buf[..res]) {
-            Some(key) => key,
-            None => "< None >".to_string(),
-        };
-        print!(" ‖ {name:^-width$}", name = key_name, width = column_width);
-    }
-
-    // Print simple footer
-    println!("‖\n {}", "-".repeat(total_width));        
-}
\ No newline at end of file
diff --git a/src/pedal_operations.rs b/src/pedal_operations.rs
index 40d8e95..14693da 100644
--- a/src/pedal_operations.rs
+++ b/src/pedal_operations.rs
@@ -1,9 +1,11 @@
+#[path = "key_operations.rs"] mod key_operations;
+
 extern crate hidapi;
 
 pub struct PedalsData {
     header: [u8; 8],
     data: [u8; 48],
-    pub length: i32,
+    length: i32,
 }
 
 pub struct Pedals {
@@ -47,16 +49,62 @@ impl Pedals {
         }
     }
 
-    fn write_pedal(&self, dev: & hidapi::HidDevice, pedal: & PedalsData) {
-        dev.write(&pedal.header).unwrap();
+    fn read_pedal(&self, dev: & hidapi::HidDevice, ped:u8) -> [u8; 8] {
+        let mut buf = [0u8; 8];
+        let mut query = [0x01u8, 0x82, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00];
+
+        query[3] += ped;
+
+        dev.write(&query).unwrap();
+
+        dev.read(&mut buf[..]).unwrap();
+
+        buf
+    }
+
+    /// Read the current values of the pedals
+    pub fn read_pedals(&self, dev: & hidapi::HidDevice) {
+        let column_width = 15;
+        let total_width = (column_width+3)*3;
+
+        // Print header
+        println!(" {}", "-".repeat(total_width));        
+        println!(" ‖{name:^width$}‖", name = "Programmed Keys", width = total_width - 2);
+        println!(" {}", "-".repeat(total_width));        
+
+        // Read and print keys
+        for i in 0..3 {
+            let key_name = match key_operations::print_key(&self.read_pedal(dev, i)) {
+                Some(key) => key,
+                None => "< None >".to_string(),
+            };
+            print!(" ‖ {name:^-width$}", name = key_name, width = column_width);
+        }
+
+        // Print simple footer
+        println!("‖\n {}", "-".repeat(total_width));        
+    }
+
+    fn write_pedal(&self, dev: & hidapi::HidDevice, ped:usize) {
+        // First, write header
+        dev.write(&self.ped_data[ped].header).unwrap();
+
+        // Write data to device in 8 byte chunks
+        for i in 0..self.ped_data[ped].length {
+            // Set bounds
+            let low:usize = i as usize;
+            let up:usize = 8*(i+1) as usize;
+
+            // Write to device
+            dev.write(&self.ped_data[ped].data[low..up]).unwrap();
+        }
     }
 
     pub fn write_pedals(&self, dev: & hidapi::HidDevice) {
         dev.write(&self.start).unwrap();
 
-        for pedal in self.ped_data.iter() {
-            self.write_pedal(dev, &pedal)
+        for (i, _pedal) in self.ped_data.iter().enumerate() {
+            self.write_pedal(dev, i)
         }
     }
-      
 }