main.cpp

00001 #include "mbed.h"
00002 #include "SRF05.h"   
00003 #include "Simplifier.h"
00004 #include "Shape.h"
00005 #include "VL53L0XSimplifier.h"
00006 
00007 #define Pin1 PB_0  
00008 #define Pin2 PB_0  
00009 #define Pin_GPIO PB_0  
00010 //DigitalOut myled(LED1);
00011 
00012 PwmOut Drive_pin_A(PA_8);  //steering servo PWM output pin D7
00013 PwmOut Drive_pin_B(PA_9);  //Not Used
00014 PwmOut Drive_pin_C(PB_10); //Motor drive PWM output pin D6
00015 PwmOut Drive_pin_D(PB_4);  //Motor drive PWM output pin D5    
00016 
00017 int Turn_right=2;
00018 int Turn_left=0;
00019 int Speed1=10;              //Half power
00020 int Speed2=20;              //Full power
00021 int PW_Period=20; 
00022 
00023 int main()
00024 { 
00025     Simplifier::accelerator();
00026     VL53Simplifier::runVL53();
00027   // default main
00028    Drive_pin_D.period_ms(PW_Period);    
00029     Drive_pin_D.pulsewidth_ms(0);        
00030     Drive_pin_C.period_ms(PW_Period);    
00031     Drive_pin_C.pulsewidth_ms(0);        //drive motor off
00032     
00033     wait_us(7);
00034     
00035     Drive_pin_A.period_ms(PW_Period);   //Steering Servo period
00036     Drive_pin_A.pulsewidth_us(1525);   //Steering neutral (includes a 25uS offset)
00037       
00038     wait_us(1);
00039     
00040     Drive_pin_D.period_ms(PW_Period);    
00041     Drive_pin_D.pulsewidth_ms(0);
00042     Drive_pin_C.period_ms(PW_Period);
00043     Drive_pin_C.pulsewidth_ms(Speed1);    //drive motor on forward half power
00044     
00045     wait_us(4);
00046     Shape::oval();
00047 
00048     Drive_pin_A.pulsewidth_us(1275); //Half turn right
00049 
00050     wait_us(7 );
00051   
00052     Drive_pin_A.pulsewidth_us(1000); // Full steering lock
00053     Drive_pin_C.period_ms(PW_Period);
00054     Drive_pin_C.pulsewidth_ms(Speed2);   // Full power
00055     
00056     wait_us(5);
00057     
00058     Drive_pin_A.pulsewidth_us(1525); // Neutral steering 
00059     
00060     wait_us(1);
00061     
00062     Drive_pin_A.pulsewidth_us(2000); // full steering opposite lock
00063     
00064     wait_us(5.5);
00065     
00066     Drive_pin_A.pulsewidth_us(1525); // Neutral steering 
00067     
00068     wait_us(1);
00069     
00070     Drive_pin_D.period_ms(PW_Period);    //drive motor reverse
00071     Drive_pin_D.pulsewidth_ms(Speed1);
00072     Drive_pin_C.period_ms(PW_Period);
00073     Drive_pin_C.pulsewidth_ms(0);    
00074     
00075     wait_us(3);
00076     
00077     Drive_pin_C.period_ms(PW_Period);    //drive motor off
00078     Drive_pin_C.pulsewidth_ms(0);
00079     Drive_pin_D.period_ms(PW_Period);
00080     Drive_pin_D.pulsewidth_ms(0);   
00081     
00082 }
00083