Diff: main.cpp

Revision:

8:a1067fcde341

Parent:

7:27516a2b504b

--- a/main.cpp	Sun Apr 17 01:07:25 2016 +0000
+++ b/main.cpp	Sat Apr 23 03:55:39 2016 +0000
@@ -13,12 +13,13 @@
 #define INITIAL_D 0.000233453623255507
 #define INITIAL_N 51.0605584484153
 #define BRAKE_CONSTANT 40
-#define BRAKE_WAIT 0.3
+#define BRAKE_WAIT 2
 #define GYRO_OFFSET 0.0152
 #define END_THRESH 4
 #define START_THRESH 10
 #define MINIMUM_VELOCITY 15
 
+
 Serial ser(USBTX, USBRX);    // Initialize Serial port
 PwmOut motor(PTD1);         // Motor connected to pin PTD1
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
@@ -47,37 +48,56 @@
 float processMagAngle();
 void magCal();
 
-// State variables
-float feedback, velocity = 0;
+// State variables and functions
+float feedback, velocity = 0, jog_duty_cycle, jog_period;
+bool alternate_motor = 0;
 void readProtocol();
 void brakeMotor();
 void reverseMotor(int speed);
 void setVelocity(int new_velocity);
+void motorAlternation();
+Ticker interruption;
 
 // Test functions
 void debug();
 
 int main(){
-    printf("Initializing controller....\r\n\r\n");
+    //printf("Initializing controller....\r\n\r\n");
     initializeController();
-    printf("Controller Initialized. \r\n");
-    magCal();
-    gyro_angle = processMagAngle();
+    //printf("Controller Initialized. \r\n");
+    //magCal();
+    gyro_angle = 0;//processMagAngle();
+    ser.baud(9600);
+    ser.format(8,SerialBase::None,1);
     velocity = MINIMUM_VELOCITY;
     setMotorPWM(velocity,motor);
     startGyro();
-        while (true){
+    while (true){
         processGyroAngle();
         controlAnglePID(P,I,D,N);
-        debug();
         if(t.read_us() < Ts*1000000)
             wait_us(Ts*1000000 - t.read_us());
-        if(pc.readable())
+        if(ser.readable())
             readProtocol();
         }
 }
+void motorAlternation() {
+    interruption.detach();
+    if(!alternate_motor){
+        setMotorPWM(velocity, motor);
+        interruption.attach(&motorAlternation, jog_duty_cycle*jog_period);
+    }
+    else{
+         setMotorPWM(10, motor);
+         interruption.attach(&motorAlternation, (1-jog_duty_cycle)*jog_period);
+    }
+    alternate_motor = !alternate_motor;
+}
+ 
 void readProtocol(){
+    printf("Entrei no protocolo!\n\r");
     char msg = ser.getc();
+    printf("%d \n\r", (int)msg);
     switch(msg)
     {
         case NONE:
@@ -85,7 +105,7 @@
             return;
             break;          
         case BRAKE:
-            //ser.printf("sending green signal to led\r\n");
+            //ser.printf("BRAKE!r\n");
             brakeMotor();
             break;
         case ANG_RST:
@@ -98,15 +118,21 @@
         case ANG_REF:
             reference = get_ang_ref(ser);     
             break;
-        case GND_SPEED:
-            velocity = get_gnd_speed(ser);       
-            setMotorPWM(velocity,motor);
+        case GND_VEL:
+            //ser.printf("GND_VEL\r\n");
+            velocity = get_gnd_vel(ser);
+            //ser.printf("%f\r\n",velocity);      
+            //interruption.detach();
             break;
         case PID_PARAMS:
-            ser.putc('p');
             get_pid_params(ser, &P, &I, &D, &N);     
             break;
+        case JOG_VEL:
+            get_jog_vel(ser,&jog_period,&jog_duty_cycle);
+            interruption.attach(&motorAlternation, jog_duty_cycle*jog_period);
+            break;
         default:
+            ser.printf("GARBAGE\r\n");
            // ser.flush();
 
     }
@@ -160,6 +186,12 @@
       e[0]= e[0] - 2*PI;
     if(e[0] < -PI)
       e[0] = e[0] + 2*PI;
+    if(abs(e[0]) > PI/6){
+        setMotorPWM(MINIMUM_VELOCITY,motor);    
+    }
+    else{
+        setMotorPWM(velocity,motor);
+        }
     /* Proportinal Part */
     up[0] = e[0]*P;  
     /* Integral Part */