Diff: Motor/Motor.cpp

Revision:

12:273752f540be

Child:

14:e8cd237c8639

diff -r 7f569811a5f1 -r 273752f540be Motor/Motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp	Sat Apr 30 21:23:13 2016 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "CarPWM.h"
+#include "Motor.h"
+
+#define PI 3.141592653589793238462
+#define Ts 0.02                         // Seconds
+#define PWM_PERIOD 13.5 // ms
+#define BRAKE_CONSTANT 40
+#define BRAKE_WAIT 2
+#define END_THRESH 4
+#define START_THRESH 10
+#define MINIMUM_VELOCITY 15
+
+void Motor(){
+    PwmOut motor(PTD1);
+    }
+    
+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();
+    }
+    
+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(void){
+    stopJogging();
+    if(velocity >= 0){
+        setMotorPWM(-BRAKE_CONSTANT, 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;
+}
\ No newline at end of file