main.cpp

00001 #include "mbed.h"
00002 #include "LSM303DLH.h"
00003 
00004 DigitalOut led1(LED1);
00005 DigitalOut led2(LED2);
00006 
00007 
00008 /*
00009     Misc decl.
00010 */
00011 Serial debug(USBTX,USBRX);
00012 
00013 
00014 /*
00015     Motor Declarations and functions
00016     @param on : decidedes which bits to set from the forward and backward array 
00017 */
00018 
00019 unsigned int on = 0;
00020 
00021 DigitalOut m1(p21);
00022 DigitalOut m2(p22);
00023 DigitalOut m3(p23);
00024 DigitalOut m4(p24);
00025 
00026 enum control{
00027     forward,
00028     backward
00029 }stepper_direction = forward;
00030 
00031 float motorTimeLapsed = 0.0, degrees = 0, motorTimerFrequency = 0.007, oneRotationTime = 15.0;
00032 
00033 char* forwardArr[] = {"1000", "1100", "0100", "0110","0010", "0011","0001", "1001"};
00034 char* backwardArr[] = {"1000", "1001", "0001", "0011", "0010", "0110", "0100", "1100"};
00035 
00036 
00037 void SetPortValues(char *myValues){
00038     m1 = myValues[0] - 48;
00039     m2 = myValues[1] - 48;
00040     m3 = myValues[2] - 48;
00041     m4 = myValues[3] - 48;
00042 }
00043 
00044 
00045 void attime() {
00046     motorTimeLapsed += motorTimerFrequency;
00047     if(motorTimeLapsed > oneRotationTime){ 
00048         motorTimeLapsed = 0;
00049         stepper_direction = stepper_direction == forward?backward:forward; 
00050     }
00051     stepper_direction?SetPortValues(backwardArr[++on%8]):SetPortValues(forwardArr[++on%8]);
00052     degrees = (stepper_direction == forward)?(motorTimeLapsed/oneRotationTime*180.0):(180.0-(motorTimeLapsed/oneRotationTime*180.0));
00053 }
00054 
00055 /*
00056     Sonar - decl and functions
00057 */
00058 
00059 long duration, distance_cm;
00060 Timer timerSonar; 
00061 DigitalOut trig(p6); // To generate trigger for Sonar
00062 InterruptIn echo(p5); // To capture echo edge on I/O interrupt
00063 
00064 void begin_timer(){
00065     timerSonar.reset();  //reset timer
00066     timerSonar.start();
00067 }
00068 
00069 void end_timer()
00070 {
00071     timerSonar.stop();
00072     duration =  timerSonar.read_us(); // Pulse width measured
00073     distance_cm = (double)(duration/2)*0.034  ; 
00074 }
00075 
00076 void echo_duration() {
00077         trig=0;   // trigger low 
00078         wait_us(2); //  wait 
00079         trig=1;   //  trigger high
00080         wait_us(10);
00081         trig=0;  // trigger low
00082 }
00083 
00084 /*
00085     Magnetometer decl and functions
00086 */
00087 
00088 LSM303DLH compass(p9, p10);
00089 
00090 
00091 int main() { 
00092     //configure motor
00093     Ticker motorTimer;
00094     motorTimer.attach(&attime, motorTimerFrequency);
00095     //configure sonar
00096     echo.rise(&begin_timer);
00097     echo.fall(&end_timer);
00098     //configure compass
00099     float hdg;
00100     compass.setOffset(29.50, -0.50, 4.00);
00101     compass.setScale(1.00, 1.03, 1.21);
00102     while(1) {
00103         echo_duration();
00104         hdg = compass.heading((vector){0,-1,0});
00105         debug.printf("Degree = %f Distance = %d Heading: %.2f \n\r", degrees, distance_cm, hdg);
00106     }
00107 }