Diff: motor.h

Revision:

0:c21936a3520a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Mon Mar 23 08:38:40 2020 +0000
@@ -0,0 +1,51 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "FastPWM.h"
+
+
+
+FastPWM pwm1(MOTOR_PWM1, -1);
+FastPWM pwm2(MOTOR_PWM2, -1);
+FastPWM pwm3(MOTOR_PWM3, -1);
+FastPWM pwm4(MOTOR_PWM4, -1);
+FastPWM pwm5(MOTOR_PWM5, -1);
+//FastPWM pwm6(MOTOR_PWM6, -1);
+
+void motor_init()
+{
+    pwm1.period_us(2500);
+    pwm2.period_us(2500);
+    pwm3.period_us(2500);
+    pwm4.period_us(2500);
+    pwm5.period_us(2500);
+    //pwm6.period_us(2500);
+}
+
+void motor_power(int motor_num,double percent)
+{
+    percent=-percent;
+    double output=offset[motor_num];
+    if(percent<-500)
+        percent=-500;
+    if(percent>500)
+        percent=500;
+        
+        output = 1-(offset[motor_num] + percent)/2500;
+        
+        if(motor_num==0)
+            pwm1.write(output);
+        else if(motor_num==1)
+            pwm2.write(output);
+        else if(motor_num==2)
+            pwm3.write(output);
+        else if(motor_num==3)
+            pwm4.write(output);
+        else if(motor_num==4)
+            pwm5.write(output);
+        //else if(motor_num==5)
+        //    pwm6.write(output);
+}
+
+
+#endif
\ No newline at end of file