Revision 8:a1067fcde341, committed 2016-04-23

Comitter:

drelliak

Date:

Sat Apr 23 03:55:39 2016 +0000

Parent:

7:27516a2b504b

Commit message:

Updated what we did last weekend

Changed in this revision

--- a/Protocol/protocol.h	Sun Apr 17 01:07:25 2016 +0000
+++ b/Protocol/protocol.h	Sat Apr 23 03:55:39 2016 +0000
@@ -27,14 +27,22 @@
 #define PID_PARAMS_MIN -100.0
 #define PID_PARAMS_MAX 100.0
 
-//ground speed range
-#define GND_SPEED_MIN -100.0
-#define GND_SPEED_MAX 100.0
+//ground velocity range
+#define GND_VEL_MIN -100.0
+#define GND_VEL_MAX 100.0
 
 //angle reference range (in radians)
 #define ANG_REF_MIN -PI
 #define ANG_REF_MAX PI
 
+//jogging speed period (in seconds)
+#define JOG_VEL_PERIOD_MIN 0.0
+#define JOG_VEL_PERIOD_MAX 300.0
+
+//jogging speed ratio
+#define JOG_VEL_RATIO_MIN 0.0
+#define JOG_VEL_RATIO_MAX 1.0
+
 //messages to send via protocol
 enum
 {
@@ -46,10 +54,12 @@
     ANG_RST,
     //set new angle reference
     ANG_REF,
-    //set new ground speed for robot
-    GND_SPEED,
+    //set new ground velocity for robot
+    GND_VEL,
+    //set new jogging speed for robot
+    JOG_VEL,
     //send pid control parameters
     PID_PARAMS
 };
 
-#endif
+#endif
\ No newline at end of file

--- a/Protocol/receiver.cpp	Sun Apr 17 01:07:25 2016 +0000
+++ b/Protocol/receiver.cpp	Sat Apr 23 03:55:39 2016 +0000
@@ -61,3 +61,8 @@
 	*n = get_pid_param(serial);
 }
 
+void get_jog_vel(Serial& serial, float* period, float* ratio)
+{
+	*period = get_param(serial, JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX);
+	*ratio = get_param(serial, JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX);
+}
\ No newline at end of file

--- a/Protocol/receiver.h	Sun Apr 17 01:07:25 2016 +0000
+++ b/Protocol/receiver.h	Sat Apr 23 03:55:39 2016 +0000
@@ -47,10 +47,14 @@
 */
 void get_pid_params(Serial& serial, float* kp, float* ki, float* kd, float* n);
 
+/**
+ Gets jogging velocity.
+*/
+void get_jog_vel(Serial& serial, float* period, float* ratio);
+
 //macros just to make life easier
 #define get_pid_param(serial) get_param(serial, PID_PARAMS_MIN, PID_PARAMS_MAX)
-#define get_gnd_speed(serial) get_param(serial, GND_SPEED_MIN, GND_SPEED_MAX)
+#define get_gnd_vel(serial) get_param(serial, GND_VEL_MIN, GND_VEL_MAX)
 #define get_ang_ref(serial) get_param(serial, ANG_REF_MIN, ANG_REF_MAX)
 
-#endif
-
+#endif
\ No newline at end of file

--- 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 */