Implement speedometer and encoder

src/config.h

@@ -1,10 +1,15 @@
-//Pin Configs
-#define M_DIR_11 15
-#define M_DIR_12 0
-#define M_PWM_1 5
-#define M_DIR_21 4
-#define M_DIR_22 2
-#define M_PWM_2 18
+//Pin config
+#define M_DIR_11 27
+#define M_DIR_12 12
+#define M_PWM_1 13
+#define M_ENCODE_1A 33
+#define M_ENCODE_1B 32
+
+#define M_DIR_21 23
+#define M_DIR_22 14
+#define M_PWM_2 22
+#define M_ENCODE_2A 25
+#define M_ENCODE_2B 26
 
 //Motors
 #define LEFT_MOTOR 1
@@ -17,6 +22,7 @@
 #define BACKWARD 2
 
 //MotorControl config
+#define RUN_MOTOR_CONTROL_DELAY 300 // Normal 50 ?
 #define PWM_FREQ 16000
 #define PWM_RES 8
 #define PWM_CHANNEL_M1 0
@@ -24,4 +30,10 @@
 #define UPDATE_TIME 25
 #define SPEED_STEPS 5       //  A total of 20 levels ( 100 / SPEED_STEPS ) * UPDATE_TIME = 500ms
 #define PWM_MIN 150
-#define PWM_MAX 245         // Max 98% of 2^PWM_RES
+#define PWM_MAX 245         // Max 98% of 2^PWM_RES
+
+//Encoder config
+#define RUN_SPEEDOMETER_DELAY 500 // Normal 10 untested
+#define ENC_FILTER 2400
+#define ENC_STEPS 1024
+#define WHEEL_DIAMETER 0.1263 //meter