Revision 12:273752f540be, committed 2016-04-30

Comitter:

drelliak

Date:

Sat Apr 30 21:23:13 2016 +0000

Parent:

11:7f569811a5f1

Child:

13:f7a7fe9b5c00

Commit message:

TrekkingController with the gyroscope class and motor class

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.h	Sat Apr 30 21:23:13 2016 +0000
@@ -0,0 +1,31 @@
+#ifndef MOTOR_HEADER
+#define MOTOR_HEADER
+
+#include "mbed.h"
+#include "CarPWM.h"
+
+class Motor{
+    
+    protected:
+    Ticker interruption;
+    bool alternate_motor;
+    float velocity;
+    float jog_duty_cycle;
+    float jog_period;
+    PwmOut motor;
+
+public:
+    
+    void startJogging(float jog_dc, float jog_p);
+    void stopJogging(void);
+    void brakeMotor(void);
+    void reverseMotor(int speed);
+    void setVelocity(int new_velocity);
+    void setSmoothVelocity(int new_velocity);
+    Motor(): motor(PTD1){}
+    
+private:
+     void motorJogging(void);
+     
+};
+#endif
\ No newline at end of file

--- a/SensorsLibrary/FXAS21002.cpp	Sat Apr 30 14:18:43 2016 -0300
+++ b/SensorsLibrary/FXAS21002.cpp	Sat Apr 30 21:23:13 2016 +0000
@@ -18,13 +18,13 @@
 
 #include "FXAS21002.h"
 #include "mbed.h"
-
 FXAS21002::FXAS21002(PinName sda, PinName scl) : gyroi2c(sda,scl)
 {
    
 }
 
-void FXAS21002::set_gyro(gyro_mode mode){ // Protected method
+void FXAS21002::set_gyro(gyro_mode mode) // Protected method
+{ 
     char d[2];
     d[0] = FXAS21002_CTRL_REG1;                       //Puts device in standby mode
     d[1] = 0x08;
@@ -39,13 +39,43 @@
     gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
 }
 
+
+void FXAS21002::stop_measure(void)
+{
+    interrupt.detach();    
+}
+
+float FXAS21002::get_angle(void)
+{
+    return angle;
+}
+
+void FXAS21002::integrate_gyro_angle(void)
+{
+    float gyro_data[3];
+    acquire_gyro_data_dps(gyro_data);
+    angle = angle + (gyro_data[2]-GYRO_OFFSET)*(period/1000000);
+    if(angle > 180)
+        angle = angle - 360;
+    if(angle < -180)
+        angle = angle + 360;
+}
+
+void FXAS21002::start_measure(float period_us)
+{
+    period = period_us;
+    interrupt.attach_us(this,&FXAS21002::integrate_gyro_angle,period);
+    angle = 0;
+}
+
 void FXAS21002::gyro_config(void)
 {
     set_gyro(MODE_2);           //Default implementation
     sensitivity = 0.03125;
 }
  
-void FXAS21002::gyro_config(gyro_mode mode){
+void FXAS21002::gyro_config(gyro_mode mode)
+{
     set_gyro(mode);
     switch(mode)
     {

--- a/SensorsLibrary/FXAS21002.h	Sat Apr 30 14:18:43 2016 -0300
+++ b/SensorsLibrary/FXAS21002.h	Sat Apr 30 21:23:13 2016 +0000
@@ -27,7 +27,7 @@
 #define FXAS21002_CTRL_REG0 0x0D
 #define FXAS21002_CTRL_REG1 0x13
 #define FXAS21002_WHO_AM_I_VALUE 0xD1
-
+#define GYRO_OFFSET 0.0093
 /*  Gyroscope mechanical modes
 *   Mode    Full-scale range [Deg/s]    Sensitivity [(mDeg/s)/LSB]
 *   1       +- 2000                     62.5
@@ -41,7 +41,7 @@
     MODE_2 = 0x01,
     MODE_3 = 0x02,
     MODE_4 = 0x03
-}
+};
 
 class FXAS21002
 {
@@ -53,10 +53,18 @@
     void gyro_config(gyro_mode mode);
         
     void acquire_gyro_data_dps(float * du);
+    void start_measure(float period_us);
+    void stop_measure(void);
+    float get_angle(void);
     
     private:
+    void set_gyro(gyro_mode mode);
+    void integrate_gyro_angle(void);
+    Ticker interrupt;
     I2C gyroi2c;
-    float sensivity;
+    float sensitivity;
+    float angle;
+    float period;
 };
 
 #endif
\ No newline at end of file

--- a/main.cpp	Sat Apr 30 14:18:43 2016 -0300
+++ b/main.cpp	Sat Apr 30 21:23:13 2016 +0000
@@ -3,24 +3,23 @@
 #include "mbed.h"
 #include "CarPWM.h"
 #include "receiver.h"
-
+#include "Motor.h"
 
 #define PI 3.141592653589793238462
 #define Ts 0.02                         // Seconds
-#define PWM_PERIOD 13.5 // ms
+#define PWM_PERIOD 13.5                 // ms
 #define INITIAL_P 0.452531214933414
 #define INITIAL_I 5.45748932024049
 #define INITIAL_D 0.000233453623255507
 #define INITIAL_N 51.0605584484153
 #define BRAKE_CONSTANT 40
 #define BRAKE_WAIT 0.3
-#define GYRO_OFFSET 0.0152
 #define END_THRESH 4
 #define START_THRESH 10
 #define MINIMUM_VELOCITY 15
+#define GYRO_PERIOD 1300                //us
 
 Serial ser(USBTX, USBRX);    // Initialize Serial port
-PwmOut motor(PTD1);         // Motor connected to pin PTD1
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
 
 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
@@ -34,13 +33,6 @@
 void controlAnglePID(float P, float I, float D, float N);
 void initializeController();
 
-// Gyroscope variables and functions
-float gyro_data[3], gyro_angle;
-Timer t; 
-void processGyroAngle();
-void startGyro();
-void stopGyro();
-
 // Magnetometer variables and functions
 float max_x, max_y, min_x, min_y,x,y;
 MotionSensorDataUnits mag_data;
@@ -48,65 +40,21 @@
 void magCal();
 
 // State variables
-float feedback, velocity = 0;
+float velocity = 0;
 void readProtocol();
 void brakeMotor();
 void reverseMotor(int speed);
 void setVelocity(int new_velocity);
 
-// Test functions
-void debug();
-
 int main(){
-    int teste = 0;
-    setVelocity(0);
-    switch(teste){
-    case 0: // vai para tras duas vezes
-        brakeMotor();
-        wait(1);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        break;
-    case 1: // vai para a frente e depois para tras
-        brakeMotor();
-        wait(1);
-        setVelocity(20);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        break;
-    case 2: // vai para tras e depois para frente
-        brakeMotor();
-        wait(1);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(20);
-        break;
-    case 3: // vai para frente duas vezes
-        brakeMotor();
-        wait(1);
-        setVelocity(20);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(20);
-        break;
+    gyro.gyro_config(MODE_2);
+    gyro.start_measure(GYRO_PERIOD);
+    initializeController();
+    while(1){
+        controlAnglePID(P,I,D,N);
+        printf("%f \r\n",gyro.get_angle());
+        wait(Ts);
     }
-    while(1);
 }
 void readProtocol(){
     char msg = ser.getc();
@@ -122,9 +70,8 @@
             break;
         case ANG_RST:
             //ser.printf("sending blue signal to led\r\n");
-            stopGyro();
-            gyro_angle = 0;
-            startGyro();
+            gyro.stop_measure();
+            gyro.start_measure(GYRO_PERIOD);
             return;
             break;
         case ANG_REF:
@@ -156,36 +103,10 @@
     N= INITIAL_N;
 }
 
-/* Start the Gyroscope timer and set the initial configuration */
-void startGyro(){
-    gyro.gyro_config(); 
-    t.start();
-}
-
-/* Stop and reset the Gyroscope */
-void stopGyro(){
-    t.stop();
-    t.reset();
-    gyro_angle = 0;
-}
-
-/* Integrate the Gyroscope to get the angular position (Deegre/seconds) */
-void processGyroAngle(){
-    gyro.acquire_gyro_data_dps(gyro_data);
-    t.stop();
-    gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
-    t.reset();
-    t.start();
-    feedback = gyro_angle;
-    if(feedback > 180)
-        feedback = feedback - 360;
-    if(feedback < -180)
-        feedback = feedback + 360;
-}
-
 /* PID controller for angular position */
 void controlAnglePID(float P, float I, float D, float N){ 
     /* Getting error */
+    float feedback = gyro.get_angle();
     e[1] = e[0];
     e[0] = reference - (feedback*PI/180);
     if(e[0] > PI)
@@ -252,14 +173,6 @@
         velocity--;
         }
 }
-/* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used    */
-/* in the main loop or the controller performance may be affected.                                                      */
-void debug(){
-    //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
-    //printf("Erro: %f  Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
-    printf(" %f \r\n",feedback);
-}
-
 /* Function to normalize the magnetometer reading */
 void magCal(){
         printf("Starting Calibration");