Diff: main.cpp

Revision:

0:9d2ceb7b8ec9

diff -r 000000000000 -r 9d2ceb7b8ec9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 20 04:32:01 2015 +0000
@@ -0,0 +1,192 @@
+#include "mbed.h"                                              
+#include "LSM9DS0.h"                                          
+#include "math.h"                                               
+
+LSM9DS0 imu(p9, p10, 0x6B, 0x1D); 
+Serial pc(USBTX, USBRX);
+Ticker control;  // create Ticker for control algorithm
+Ticker db;
+
+//debugging
+int debug=0;
+int wt = 500;
+DigitalOut led1(LED1);
+
+//motor variables
+PwmOut left_motor_speed(p21); //left motor pwm A
+PwmOut right_motor_speed(p22); //right motor pwm B
+DigitalOut left_fwd_enable(p23); //fwd enable of left/A motor
+DigitalOut left_rev_enable(p24); //rev enable of left/A motor
+DigitalOut right_fwd_enable(p26); //fwd enable or right/B motor
+DigitalOut right_rev_enable(p25); //rev enable of right/B motor
+
+//angle variables
+float accelerometer_angle =0;
+float gyro_data =0;
+float gyro_angle=0;
+float current_angle =0;
+float angle_offset = 0;
+int i=0;
+
+//control variables
+float Error=0; // I had to capitalize this for some reason unlike the other vars
+float kp=.5;
+float ki=.01;
+float kd= 0;
+float setpoint=0; //reference angle
+float angle=0;
+float integral=0;
+float derivative = 0;
+float dt= 0.01; //time period 1/100 (s)
+float previous_error =0;
+float actuator_input =0;
+int ready = 0;
+float maximum_angle=15;
+float SCALAR = 0;
+
+void initialiation (void)
+{
+    //with the "initialization" in the control loop, we may not use this
+    //this is where offsets and sensors are set up
+    //need to hold robot balanced at this point
+    imu.readGyro();
+    angle_offset= float(imu.gy); //maybe take avg. like the other team did in a for loop   
+}
+
+void MotorDriver (float speed)
+{
+// if speed > 0 then move robot left
+// if speed < 0 then move robot right
+    if ( speed>0 )
+    {
+    //move both motors in same direction
+    right_rev_enable = 0;
+    right_fwd_enable = 1;
+    left_fwd_enable = 0;
+    left_rev_enable = 1;
+    right_motor_speed = speed;
+    left_motor_speed = speed;
+    //pc.printf("moving f\n");
+    }
+    else if ( speed <0 )
+    {
+    //move both motors in same direction
+    right_fwd_enable = 0;
+    right_rev_enable = 1;
+    left_rev_enable = 0;
+    left_fwd_enable = 1;
+    right_motor_speed = -speed;
+    left_motor_speed= -speed;
+    //pc.printf("moving r\n");
+    }
+    else 
+    {
+    right_fwd_enable = 0;
+    right_rev_enable = 0;
+    left_rev_enable = 0;
+    left_fwd_enable = 0;
+    right_motor_speed = speed;
+    left_motor_speed = speed;
+    //pc.printf("not moving\n");
+    }
+}
+
+float calcSCALAR(float degrees)
+{
+    //give it a degree such as 10 degrees, anything past that is the same as 10 degrees
+    Error = 0 - degrees; //this is the maximum error
+    integral = integral + (Error*dt); //Forward Euler approximation
+    derivative = (Error - previous_error) / dt; //change wrt time
+    previous_error = Error; 
+    SCALAR = ( (Error*kp) + (integral*ki) + (derivative*kd) );
+    pc.printf("here in scalar calc and error is %f and integral is %f and derivative is %f \n", Error, integral, derivative);
+    Error=0;
+    integral=0;
+    derivative=0;
+    previous_error=0;    
+    return 1;
+}
+
+void controlalgorithm (void)
+{
+    if (i< wt) //gives you time to balance it upon reset; this is like an initialization code for the imu;
+    {
+        //determine current angle using a complementary filter found online http://www.pieter-jan.com/node/11
+        //place +z-axis up, + x-axis right, and +y-axis away from the back of the robot when robot can move left to right 
+        imu.readGyro();
+        gyro_data= imu.gy;
+        imu.readAccel(); 
+        accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW; this gives the angle as determined by the accelerometer
+        current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);
+        i=i+1;      
+    }
+    
+    //if the SCALAR has been calculated and the initilization complete, start controlling
+    if ( ready ==1 )
+    {
+        imu.readGyro();
+        gyro_data= imu.gy; //maybe negative?? I don't think so
+        imu.readAccel(); 
+        accelerometer_angle = atan2(imu.ax, imu.az) * 180 / 3.14159; //angle in degrees; positive angle is CCW, negative angle is CW
+        current_angle = 0.98 * (current_angle + gyro_data * dt) + 0.02 * (accelerometer_angle);    
+            
+        //maybe take into account integral windup
+        Error = setpoint - current_angle; //determine the difference in angle
+        integral = integral + (Error*dt); //Forward Euler approximation
+        derivative = (Error - previous_error) / dt; //change wrt time
+        previous_error = Error; 
+        actuator_input = ( (Error*kp) + (integral*ki) + (derivative*kd) ) / SCALAR; //we should scale this???? and write in terms of a PWM value [-1,1]; be careful, SCALAR is negative
+        MotorDriver(actuator_input);
+    }
+    
+}
+
+void debugout (void)
+{
+    //pc.printf ("setpoint is %f\n", setpoint);
+    pc.printf ("actuator_input is %f\n", actuator_input);
+    //pc.printf("Speed is: %f, angle is: %f\n\n",speed,current_angle);
+}
+
+main()
+{
+    pc.printf("starting...\n");
+    uint16_t status = imu.begin();
+    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
+    pc.printf("Should be 0x49D4\n\n");
+    
+    SCALAR= calcSCALAR(maximum_angle); //scale the actuator inputl depends on lp, kd, and ki, and maximum angle allowed
+    control.attach(&controlalgorithm, dt); //check frequency, 100 Hz
+    if (debug==1)
+    {
+        db.attach(&debugout, 0.1);
+    }
+    
+    while(1)
+    {
+        //put some calcs here if needed; need 0.01 seconds at least
+        if (i==wt)
+        {
+           // setpoint=current_angle / 1000; //take first 1000 samples for current angle then avg. it
+            setpoint=current_angle; 
+            ready=1;
+            i=i+1;
+            pc.printf ("i is %d in top if statement\n\n\n\n", i);
+        }
+        
+        //if (debug == 1 && i>20 )
+//        {
+//            //probably need some kind of wait statement; will a wait statement affect the timer interrupt? no, i checked
+//            pc.printf("accel: x: %f y: %f z: %f\n",imu.ax,imu.ay,imu.az);
+//            pc.printf("gyro: x: %f y: %f z: %f\n",imu.gx,imu.gy,imu.gz);
+//            pc.printf ("current_angle is %f\n", current_angle);
+//            pc.printf ("setpoint is %f\n", setpoint);
+//            pc.printf ("accelerometer_angle is %f\n", accelerometer_angle);
+//            pc.printf ("gyro_angle is %f\n", gyro_angle);
+//            pc.printf ("actuator_input is %f\n", actuator_input);
+//            pc.printf ("SCALAR is %f \n", SCALAR);
+//            pc.printf ("i is %d\n\n\n\n", i);
+//            wait(2);
+//        }
+    }
+}