Revision 9:36807f7e58fb, committed 2016-05-02
- Comitter:
- wschon
- Date:
- Mon May 02 04:33:43 2016 +0000
- Parent:
- 8:198568d5746b
- Child:
- 10:72363392ecb5
- Commit message:
- f
Changed in this revision
--- a/LSM9DS1_Library_cal.lib Sun May 01 23:56:41 2016 +0000
+++ b/LSM9DS1_Library_cal.lib Mon May 02 04:33:43 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/4180_1/code/LSM9DS1_Library_cal/#36abf8e18ade
+https://developer.mbed.org/users/4180_1/code/LSM9DS1_Library_cal/#4dbf62f1395d
--- a/main.cpp Sun May 01 23:56:41 2016 +0000
+++ b/main.cpp Mon May 02 04:33:43 2016 +0000
@@ -12,7 +12,7 @@
AnalogIn infraredF(p17); //Front infrared distance sensor
Serial pc(USBTX, USBRX);
Timer t;
-char str[40]; //buffer for xbee messages
+char str[60]; //buffer for xbee messages
float maxspeed = 0.4;
float minspeed = -0.4;
float leftDist, rightDist, frontDist; //Distances from robot to obstacles
@@ -20,11 +20,19 @@
float xmag, ymag, zmag, head; //Magnetic readings
int ldist, rdist, fdist;
Serial xbee(p28,p27); //xbee over serial
-
+Mutex stdio_mutex;
+float heading;
//dual H bridge Polulu
Motor MotorRi(p21, p23, p24);
Motor MotorLe(p22, p26, p25);
+float ti = 0, timelast = 0, deltat;
+float imu[] = {0, 0, 0};
+float velX[] = {0, 0, 0};
+float velY[] = {0, 0, 0};
+float posX[] = {0, 0, 0};
+float posY[] = {0, 0, 0};
+float compass[] = {0, 0, 0};
void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
@@ -32,7 +40,6 @@
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
mx = -mx;
- float heading;
if (my == 0.0)
heading = (mx < 0.0) ? 180.0 : 0.0;
else
@@ -51,7 +58,19 @@
//pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
//pc.printf("Magnetic Heading: %f degress\n\r",heading);
- head = heading;
+ head = heading * PI / 180.0;
+}
+
+void shiftArrays() {
+ int i;
+ for (i = 2; i > 0; i--) {
+ posX[i] = posX[i - 1];
+ posY[i] = posY[i - 1];
+ velX[i] = velX[i - 1];
+ velY[i] = velY[i - 1];
+ imu[i] = imu[i - 1];
+ compass[i] = compass[i-1];
+ }
}
void IMU_thread(void const *args) {
@@ -61,11 +80,11 @@
pc.printf("Failed to communicate with LSM9DS1.\n");
}
IMU.calibrate(1);
- pc.printf("calibrated accel, now do mag\r\n");
+ //pc.printf("calibrated accel, now do mag\r\n");
IMU.calibrateMag(0);
- pc.printf("calibrated mag\r\n");
+ //pc.printf("calibrated mag\r\n");
while(1) {
- pc.printf("check accelAvailable\r\n");
+ //pc.printf("check accelAvailable\r\n");
while(!IMU.accelAvailable());
//pc.printf("got accel\r\n");
IMU.readAccel();
@@ -73,14 +92,27 @@
//pc.printf("got mag\r\n");
IMU.readMag();
xaccel = IMU.calcAccel(IMU.ax);
+ if (abs(xaccel) < 0.13)
+ xaccel = 0.0;
yaccel = IMU.calcAccel(IMU.ay);
+ if (abs(yaccel) < 0.13)
+ yaccel = 0.0;
zaccel = IMU.calcAccel(IMU.az);
+ if (abs(zaccel) < 0.13)
+ zaccel = 0.0;
//pc.printf("%f\r\n",zaccel);
xmag = IMU.calcMag(IMU.mx);
ymag = IMU.calcMag(IMU.my);
zmag = IMU.calcMag(IMU.mz);
- //printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
- Thread::wait(500); //Wait 1/2 second
+ printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+ shiftArrays();
+ imu[0] = xaccel;
+ compass[0] = head;
+ velX[0] += imu[0]*cos(compass[0]) * deltat;
+ velY[0] += imu[0]*sin(compass[0]) * deltat;
+ posX[0] += velX[0] * deltat;
+ posY[0] += velY[0] * deltat;
+ // Thread::wait(50); //Wait
}
}
@@ -114,12 +146,15 @@
void send_thread(void const *args) {
xbee.baud(9600);
while (1) {
- //stdio_mutex.lock();
- sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%d\n", xaccel, zaccel, ymag, fdist, ldist, rdist, t.read_us()); //buffer data to be sent to computer
+ ti = t.read();
+ deltat = ti-timelast;
+ stdio_mutex.lock();
+ sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%0.6f\n", posX[0], posY[0], heading, fdist, ldist, rdist, deltat); //buffer data to be sent to computer
pc.printf(str);
- //pc.printf("%0.6f\r\n", zmag);
+ //pc.printf("%0.6f\r\n", zmag*9.8);
xbee.printf(str);
- //stdio_mutex.unlock();
+ stdio_mutex.unlock();
+ timelast = ti;
Thread::wait(500);
}
}
@@ -130,31 +165,31 @@
MotorLe.speed(-0.4);
Thread::wait(3000); //allow mag to calibrate by spinning
while(1) {
- if (fdist > 30) { //if there's room to go forward
+ if (fdist > 40) { //if there's room to go forward
if (ldist < 20) {
MotorRi.speed(0.1);
- MotorLe.speed(0.4);
- Thread::wait(80);
+ MotorLe.speed(0.3);
+ Thread::wait(40);
}
else if (rdist < 20) {
- MotorRi.speed(0.4);
+ MotorRi.speed(0.3);
MotorLe.speed(0.1);
- Thread::wait(80);
+ Thread::wait(40);
}
else {
- MotorRi.speed(0.4);
- MotorLe.speed(0.4);
+ MotorRi.speed(0.3);
+ MotorLe.speed(0.3);
}
}
else if (rdist < ldist) { //hard turn to the left
- MotorRi.speed(0.4);
- MotorLe.speed(-0.4);
- Thread::wait(200);
+ MotorRi.speed(0.3);
+ MotorLe.speed(-0.3);
+ Thread::wait(100);
}
else if (ldist < rdist) { //hard turn to the right
MotorRi.speed(-0.4);
MotorLe.speed(0.4);
- Thread::wait(200);
+ Thread::wait(100);
}
}
}
Wesley Schon
