Diff: main.cpp

Revision:

6:a37a49d2793c

Parent:

5:e6a15dcba942

--- a/main.cpp	Sun Jan 11 14:44:43 2015 +0000
+++ b/main.cpp	Fri May 01 04:59:07 2015 +0000
@@ -1,166 +1,131 @@
-//Most of the Credit goes to jimblom and Judah Okeleye
 #include "LSM9DS0.h"
 #include "mbed.h"
+#include "motordriver.h"
+
+#define pi 3.14159265359
 
 // SDO_XM and SDO_G are both grounded, so our addresses are:
 #define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
 #define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
 
-
+Ticker tmr;
 LSM9DS0 dof(p9, p10,LSM9DS0_G, LSM9DS0_XM);
 Serial pc(USBTX, USBRX); // tx, rx
+Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
+Motor right(p26, p25, p24, 1);
+InterruptIn button(p7);
 
-//Uncomment If you want to use interrupts
-/*DigitalIn pinDRDYG(p14);
-DigitalIn pinINTG(p13);
-DigitalIn pinINT1_XM(p14);
-DigitalIn pinINT2_XM(p15);*/
+DigitalOut timer_on(LED1);
+DigitalOut pb(LED2);
+DigitalOut ld3(LED3);
+DigitalOut ld4(LED4);
 
-#define PRINT_CALCULATED
-#define PRINT_SPEED 500 // 500 ms between prints
+#define R 0.06 //distance from the center of the robot to the wheel (radius)
+#define lambda_r 22 // eigenvalue fo the regulator, chosen through tests
+
+float umax=1; //maximum system input (motor speed)
 
-//Init Serial port and LSM9DS0 chip
-void setup()
-{
-    pc.baud(115200); // Start serial at 115200 bps
-    // Use the begin() function to initialize the LSM9DS0 library.
-    // You can either call it with no parameters (the easy way):
-    uint16_t status = dof.begin();
-    
-   
-   //Make sure communication is working
-    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
-    pc.printf("%x\n",status);
-    pc.printf("Should be 0x49D4\n");
-    pc.printf("\n");
-}
+float K=lambda_r*R; //control gain
+float T=0.01; //in milliseconds
 
+float w=0; //angular velocity
+float r=0; //reference for orientation
+float y=0; //current orientation
+float u=0; //difference between right and left motor speeds
+float u_star=0;
+
+float vr=umax/2; //right motor speed
+float vl=umax/2; //left motor speed
+
+int c=0; //counter for measuring time
+bool back=0; //180 degrees turn flag
+int bumps=0; //bumps counter
+int cw=-1; //turn direction flag
 
-void printGyro()
-{
-    // To read from the gyroscope, you must first call the
-    // readGyro() function. When this exits, it'll update the
-    // gx, gy, and gz variables with the most current data.
-    
-    //Uncomment code if you plan on using interrupts
-    /*while(pinDRDYG == 0)
-    {
-        ;
-    }*/
-    
-    
+//Timer ISR, called with 100Hz frequency
+void timer() {
+    //sensor input
     dof.readGyro();
-
-    // Now we can use the gx, gy, and gz variables as we please.
-    // Either print them as raw ADC values, or calculated in DPS.
-    pc.printf("G: ");
-#ifdef PRINT_CALCULATED
-    // If you want to print calculated values, you can use the
-    // calcGyro helper function to convert a raw ADC value to
-    // DPS. Give the function the value that you want to convert.
-    //pc.printf("The bias: %2f, %2f, %2f \n", dof.gbias[0], dof.gbias[1], dof.gbias[2]);
-    pc.printf("%2f",dof.calcGyro(dof.gx) - dof.gbias[0]);
-    pc.printf(", ");
-    pc.printf("%2f",dof.calcGyro(dof.gy) - dof.gbias[1]);
-    pc.printf(", ");
-    pc.printf("%2f\n",dof.calcGyro(dof.gz) - dof.gbias[2]);
-#elif defined PRINT_RAW
-    pc.printf("%d", dof.gx);
-    pc.printf(", ");
-    pc.printf("%d",dof.gy);
-    pc.printf(", ");
-    pc.printf("%d\n",dof.gz);
-#endif
+    w=(pi/15)*(dof.calcGyro(dof.gz)-dof.gbias[2]); //measure angular velocity
+    y=y+T*(w-0.0000001); //integrate to obtain orientation
+    
+    //input calculation
+    u_star=-K*(y-r);
+  
+    //limiting motor speed
+    if (u_star<-umax){
+      u=-umax;
+    } else if (u_star>umax){
+      u=umax;
+    } else{
+      u=u_star;
+    }
+    
+    //actuator output
+    vr=(umax+u)/2.0;
+    vl=(umax-u)/2.0;
+    left.speed(vr);
+    right.speed(vl);
+    
+    //one second after it bumps, reference changes to -pi or zero
+    if (c==100 && back){
+        r=r+cw*pi/2;
+        c=0;
+        back=0;
+        cw=-cw; //changes direction of 180 turn when bumps
+        pb=0;
+    }  
+    
+    c++;
 }
 
-void printAccel()
-{
-    //Uncomment code if you plan on using interrupts
-    /*while(pinINT1_XM == 0)
-    {
-        ;
-    }*/
+void bump() {
+    bumps++; //counts bumps
+    tmr.detach(); //turn off timer
+    ld4=1;
     
-
-    dof.readAccel();
+    //avoid abrupt speed changes
+    left.speed(0);
+    right.speed(0);
+    wait(0.5);
+    
+    //goes backwards for 0.5s
+    left.speed(-0.8);
+    right.speed(-0.8);
+    wait(0.5);
     
-    // Now we can use the ax, ay, and az variables as we please.
-    // Either print them as raw ADC values, or calculated in g's.
-    pc.printf("A: ");
-#ifdef PRINT_CALCULATED
-    // If you want to print calculated values, you can use the
-    // calcAccel helper function to convert a raw ADC value to
-    // g's. Give the function the value that you want to convert.
-    //pc.printf("The bias: %2f, %2f, %2f \n", dof.abias[0], dof.abias[1], dof.abias[2]);
-    pc.printf("%2f",dof.calcAccel(dof.ax) - dof.abias[0]);
-    pc.printf(", ");
-    pc.printf("%2f",dof.calcAccel(dof.ay) - dof.abias[1]);
-    pc.printf(", ");
-    pc.printf("%2f\n",dof.calcAccel(dof.az) - dof.abias[2]);
-#elif defined PRINT_RAW
-    pc.printf("%d",dof.ax);
-    pc.printf(", ");
-    pc.printf("%d",dof.ay);
-    pc.printf(", ");
-    pc.printf("%d\n",dof.az);
-#endif
+    ld4=0;  
+    pb=1;
+    c=0;
+    
+    //change reference to plus or minus pi/2
+    r=r+cw*pi/2;
     
+    //implementation of finding initial position
+    if (bumps>2){
+        ld3=1;
+        back=1;
+    }
+    
+    LPC_TIM3->TC = 0;
+    tmr.attach(&timer, T); //turn timer back on
 }
 
-void printMag()
-{
-     //Uncomment code if you plan on using interrupts
-    /*while(pinINT2_XM == 0)
-    {
-        ;
-    }*/
-    
-    // To read from the magnetometer, you must first call the
-    // readMag() function. When this exits, it'll update the
-    // mx, my, and mz variables with the most current data.
-    dof.readMag();
-    
-    // Now we can use the mx, my, and mz variables as we please.
-    // Either print them as raw ADC values, or calculated in Gauss.
-    pc.printf("M: ");
-#ifdef PRINT_CALCULATED
-    // If you want to print calculated values, you can use the
-    // calcMag helper function to convert a raw ADC value to
-    // Gauss. Give the function the value that you want to convert.
-    pc.printf("%2f",dof.calcMag(dof.mx));
-    pc.printf(", ");
-    pc.printf("%2f",dof.calcMag(dof.my));
-    pc.printf(", ");
-    pc.printf("%2f\n",dof.calcMag(dof.mz));
-#elif defined PRINT_RAW
-    pc.printf("%d", dof.mx);
-    pc.printf(", ");
-    pc.printf("%d", dof.my);
-    pc.printf(", ");
-    pc.printf("%d\n", dof.mz);
-#endif
-}
-
-void loop()
-{
-    printGyro();  // Print "G: gx, gy, gz" 
-    printAccel(); // Print "A: ax, ay, az"
-    printMag();   // Print "M: mx, my, mz"    
-    wait_ms(PRINT_SPEED);
-}
-
-
 int main()
 {
-    setup();  //Setup sensor and Serial
-    pc.printf("Hello World\n");
+    uint16_t status = dof.begin(); //start and setup gyroscope
+    dof.calLSM9DS0(dof.gbias, dof.abias); //Calculate bias
+    
+    wait(3);
+    //starts going forward with 0.5 speed in both motors
+    left.speed(vl);
+    right.speed(vr);
+    
+    button.rise(&bump); //atach bump external interrupt
+    
+    //start timer with 0.01 period
+    timer_on=1;
+    tmr.attach(&timer, T);
 
-    while (true)
-    {
-         //Calculate bias
-         dof.calLSM9DS0(dof.gbias, dof.abias);
-         //pc.printf("Setup Done\n");
-         loop(); //Get sensor values
-   }
-}
-
+    while (true){} //infinite loop
+}
\ No newline at end of file