Testing of PWM

diff --git a/src/pwm_test/motor_rt.c b/src/pwm_test/motor_rt.c
new file mode 100644 (file)
index 0000000..3d48359
--- /dev/null
+++ b/src/pwm_test/motor_rt.c
@@ -0,0 +1,84 @@
+#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);
+       }
+
+}