Diff: main.cpp

Revision:

13:f7a7fe9b5c00

Parent:

12:273752f540be

Child:

14:e8cd237c8639

--- a/main.cpp	Sat Apr 30 21:23:13 2016 +0000
+++ b/main.cpp	Sat Apr 30 21:28:27 2016 +0000
@@ -21,7 +21,7 @@
 
 Serial ser(USBTX, USBRX);    // Initialize Serial port
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
-
+Motor motor;
 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
 FXAS21002 gyro(PTE25,PTE24);
 
@@ -39,13 +39,6 @@
 float processMagAngle();
 void magCal();
 
-// State variables
-float velocity = 0;
-void readProtocol();
-void brakeMotor();
-void reverseMotor(int speed);
-void setVelocity(int new_velocity);
-
 int main(){
     gyro.gyro_config(MODE_2);
     gyro.start_measure(GYRO_PERIOD);
@@ -66,7 +59,7 @@
             break;          
         case BRAKE:
             //ser.printf("sending green signal to led\r\n");
-            brakeMotor();
+            motor.brakeMotor();
             break;
         case ANG_RST:
             //ser.printf("sending blue signal to led\r\n");
@@ -78,8 +71,7 @@
             reference = get_ang_ref(ser);     
             break;
         case GND_SPEED:
-            velocity = get_gnd_speed(ser);       
-            setMotorPWM(velocity,motor);
+            motor.setVelocity(get_gnd_speed(ser));
             break;
         case PID_PARAMS:
             ser.putc('p');
@@ -132,47 +124,6 @@
     setServoPWM(u*100/(PI/8), servo);
 }
 /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
-void brakeMotor(){
-    if(velocity >= 0){
-        setMotorPWM(-BRAKE_CONSTANT, motor);
-        wait(BRAKE_WAIT);
-        velocity = 0;
-        setMotorPWM(velocity,motor);
-    }
-    else {
-        setVelocity(0);
-    }
-}
-void 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 setVelocity(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--;
-        }
-}
 /* Function to normalize the magnetometer reading */
 void magCal(){
         printf("Starting Calibration");