Arend Peter Castelein / Alexa_Inventory

Dependencies:   LSM9DS1_Library_cal Motordriver

main.cpp

Committer:
apcastelein
Date:
2016-11-29
Revision:
3:b65b394375b7
Parent:
2:c91ce1f091f5
Child:
4:104acd180bb1

File content as of revision 3:b65b394375b7:

#include "mbed.h"
#include "LSM9DS1.h"
#define PI 3.14159

//IMU used for monitoring orientation
// - wired using i2c in cookbook example here
//   https://developer.mbed.org/components/LSM9DS1-IMU/
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);

//pc serial connection, for debugging purposes
Serial pc(USBTX, USBRX);

//initialize and calibrate IMU
void initIMU(){
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0);   
}

//initialization for all components
void init(){
    initIMU();    
}

// Sends a request to the server and waits for a path response
char* getPathFromServer(){
    //use wifi module here
    //https://developer.mbed.org/users/electromotivated/notebook/wifi-pid-redbot-robot-webserver/    
    char* path = "";
    return path;
}

//gets the orientation from imu
float getDirection(){
    float sum = 0;
    int target_samples = 100;
    int num_samples = 0;
    while(num_samples++ < target_samples){
        while(!IMU.magAvailable(X_AXIS));
            IMU.readMag();
        float my = IMU.calcMag(IMU.my);
        float mx = IMU.calcMag(IMU.mx);
        float dir;
        if (my == 0.0)
            dir = (mx < 0.0) ? 180.0 : 0.0;
        else
            dir = atan2(mx, my)*360.0/(2.0*PI);
        sum += dir;
    }
    return sum / target_samples;
}

//technologies for navigation may include
// - Line following https://www.sparkfun.com/products/11769
// - color sensor (indicates when end of movement is reached)
void moveForward(){}
void rotateLeft(){}
void rotateRight(){}

//arem technology is yet to be determined, will probably involve adjusting a motor
void armUp(){}
void armDown(){}

// Loops through path commands and executes each one sequentially
void executePath(char* path){
    int index = 0;
    while(path[index] != '\0'){
        switch(path[index]){
            case 'F': moveForward(); break;    
            case 'L': rotateLeft(); break;
            case 'R': rotateRight(); break;
            case 'U': armUp(); break;
            case 'D': armDown(); break;
        }    
    }
}

//main code
//- repeatedly listens for paths and then executes them
int main() {
    init();
    while(true){
        char* path = getPathFromServer(); 
        executePath(path);
    }
}