Diff: Motor/Motor.cpp

Revision:

0:8f5db5085df7

diff -r 000000000000 -r 8f5db5085df7 Motor/Motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,88 @@
+
+#include "Motor.hpp"
+
+#define PI 3.141592653589793238462
+#define PWM_PERIOD 13.5                 // ms
+#define BRAKE_CONSTANT 100              // Brake force(max = 100)
+#define BRAKE_WAIT 1.662                // seconds
+#define MINIMUM_VELOCITY 15
+
+Motor::Motor(PinName motor_pin): motor(motor_pin){}
+
+    
+void Motor::startJogging(float jog_dc, float jog_p){
+    jog_duty_cycle = jog_dc;
+    jog_period = jog_p;
+    interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+}
+    
+void Motor::stopJogging(void){
+    interruption.detach();
+    setMotorPWM(velocity,motor);
+}
+    
+void Motor::motorJogging(void) {
+    interruption.detach();
+    if(!alternate_motor){
+        setMotorPWM(velocity, motor);
+        interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+    }
+    else{
+         setMotorPWM(10, motor);
+         interruption.attach(this,&Motor::motorJogging, (1-jog_duty_cycle)*jog_period);
+    }
+    alternate_motor = !alternate_motor;
+}
+
+void Motor::brakeMotor(float brake_intensity, float brake_wait){
+    stopJogging();
+    if(velocity >= 0){
+        setMotorPWM(-brake_intensity, motor);
+        wait(brake_wait);
+        velocity = 0;
+        setMotorPWM(velocity,motor);
+    }
+    else {
+        setVelocity(0);
+    }
+}
+
+void Motor::reverseMotor(int speed){
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=-speed ; i <= 0; i++){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+    for(int i=0 ; i >= -speed; i--){
+        setMotorPWM((float)i,motor);
+        wait_ms(13.5);    
+    }
+}
+
+void Motor::setSmoothVelocity(int new_velocity){
+    if( velocity > new_velocity){
+        for(; velocity >= new_velocity; velocity--){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity++;
+    }
+    else if(velocity < new_velocity){
+        for(; velocity <= new_velocity; velocity++){
+            setMotorPWM(velocity,motor);
+            wait_ms(PWM_PERIOD);
+            }
+        velocity--;
+    }
+}
+
+void Motor::setVelocity(int new_velocity){
+ setMotorPWM(new_velocity,motor);
+ velocity = new_velocity;
+}
+float Motor::getVelocity(){
+    return velocity;
+}
\ No newline at end of file