--- /dev/null
+#include <stdio.h>
+#include <time.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <pthread.h>
+#include <signal.h>
+
+int led, motor_A, motor_B, motor_active, i;
+struct timespec req1 = {0};
+struct timespec req2 = {0};
+int base_task_prio;
+
+
+void turn(int duty, int direction){
+ pthread_t thread_id;
+
+ if(direction > 0){
+ motor_active = motor_A;
+ write(motor_B,"0",1);
+ }
+ else{
+ motor_active = motor_B;
+ write(motor_A,"0",1);
+ }
+ req1.tv_nsec = duty * 500L;
+
+ req2.tv_nsec = (100 - duty) * 500L;
+
+ for(i = 0; i < 20000;i++){
+ write(motor_active,"1",1);
+ nanosleep(&req1, (struct timespec *)NULL);
+ write(motor_active,"0",1);
+ nanosleep(&req2, (struct timespec *)NULL);
+ }
+
+}
+
+
+void main(){
+ int fifo_min_prio = sched_get_priority_min(SCHED_FIFO);
+ int fifo_max_prio = sched_get_priority_max(SCHED_FIFO);
+ pthread_attr_t attr;
+ struct sched_param schparam;
+ if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) != 0) {
+ fprintf(stderr, "pthread_attr_setinheritsched failed\n");
+ }
+
+ if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO) != 0) {
+ fprintf(stderr, "pthread_attr_setschedpolicy SCHED_FIFO failed\n");
+ }
+
+ base_task_prio = fifo_max_prio - 20;
+ if (base_task_prio < fifo_min_prio){
+ base_task_prio = fifo_min_prio;
+ }
+
+ schparam.sched_priority = base_task_prio;
+
+ motor_A = open("/sys/class/gpio/gpio919/value", O_WRONLY);
+ motor_B = open("/sys/class/gpio/gpio916/value", O_WRONLY);
+ led = open("/sys/class/gpio/gpio953/value", O_WRONLY);
+
+ if((motor_A < 0) || (motor_B < 0)){
+ printf("Motor export failed!\n");
+ }
+ write(led, "1", 1);
+
+ while(1){
+ turn(10, 1);
+ turn(100, 1);
+ turn(0, 1);
+ turn(5, -1);
+ turn(100, -1);
+ turn(0, -1);
+ }
+
+}