main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  Julien Tiron, FIP Télécom Bretagne
00005  * @version V1.0.0
00006  * @date    March 23th, 2016
00007  * @brief   DoorCloser robot main code
00008  ******************************************************************************
00009  **/
00010 
00011 /* Includes ------------------------------------------------------------------*/
00012 
00013 #include "mbed.h"
00014 #include "DevSPI.h"
00015 #include "l6474_class.h"
00016 #include "DevI2C.h"
00017 #include "VL6180x.h"
00018 #include "AX12.h"
00019 #include "Servo.h"
00020 /* Definitions ---------------------------------------------------------------*/
00021 
00022 #define VL6180X_ADDRESS 0x29
00023 
00024 /* Variables -----------------------------------------------------------------*/
00025 
00026 /* Start and  Stop Component */
00027 InterruptIn startup(PC_1);
00028 Ticker pwm_ticker;
00029 Timeout game_length;
00030 volatile bool start = 1;
00031 volatile bool end = 1;
00032 char data = 0x08 | (char)64;
00033 bool tag = true;
00034 int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
00035 int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
00036 Servo myservo(D5);
00037 PwmOut parasol(A3);
00038 /* Motor Control Component */
00039 L6474 *motor1;
00040 L6474 *motor2;
00041 DigitalInOut sdaDummy(D14);
00042 DigitalInOut sclDummy(D15);
00043 /* Distance Sensors Component */
00044 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
00045 
00046 /* Functions -----------------------------------------------------------------*/
00047 
00048 void go()
00049 {
00050     start = 0;
00051 }
00052 
00053 void stop()
00054 {
00055     end = 0;
00056 }
00057 
00058 void init_sensor()
00059 {
00060 
00061 }
00062 
00063 void switch_sensor(int number)
00064 {
00065 
00066 }
00067 
00068 void pwm();
00069 
00070 void deplacement(int distanceEnCm, bool sens)
00071 {
00072     int nbPas = distanceEnCm*166.7;
00073     if(sens) {
00074         motor1->Move(StepperMotor::BWD,nbPas);
00075         motor2->Move(StepperMotor::FWD,nbPas);
00076     } else {
00077         motor1->Move(StepperMotor::FWD,nbPas);
00078         motor2->Move(StepperMotor::BWD,nbPas);
00079     }
00080     motor1->WaitWhileActive();
00081     motor2->WaitWhileActive();
00082     motor1->HardStop();
00083     motor2->HardStop();
00084 }
00085 
00086 void rotation(int angleEnDegre, int diametre)
00087 {
00088     int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
00089     int nbPas = 160*deplacementEnCm;
00090     nbPas /= 990;
00091 
00092     /*
00093     int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
00094     int nbPas = 160*deplacementEnCm;
00095     nbPas *= 2;*/
00096 
00097     printf("%d \n\r", nbPas);
00098     motor2->Move(StepperMotor::FWD,nbPas);
00099     motor1->Move(StepperMotor::FWD,nbPas);
00100     motor1->WaitWhileActive();
00101     motor2->WaitWhileActive();
00102     motor1->HardStop();
00103     motor2->HardStop();
00104 }
00105 
00106 
00107 /* Main ----------------------------------------------------------------------*/
00108 
00109 int main()
00110 {
00111     /*----- Initialization. -----*/
00112 
00113     /* Initializing SPI bus. */
00114     DevSPI dev_spi(D11, D12, D13);
00115     myservo.calibrate(0.00095, 90.0); // Calibrate the servo
00116     parasol.period_ms(10);
00117     parasol.pulsewidth_ms(1);
00118     /* Initializing Motor Control Components. */
00119     motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
00120     motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
00121     if (motor1->Init() != COMPONENT_OK)
00122         exit(EXIT_FAILURE);
00123     if (motor2->Init() != COMPONENT_OK)
00124         exit(EXIT_FAILURE);
00125     sdaDummy.mode(PullUp);
00126     sclDummy.mode(PullUp);
00127     //pwm_ticker.attach(&pwm, 0.1);
00128     /* Initializing AX-12 motors */
00129     /*    AX12 ax12_1 (D1, D0, 1, 1000000);
00130         AX12 ax12_2 (D1, D0, 2, 1000000);
00131         AX12 ax12_3 (D1, D0, 3, 1000000);
00132         AX12 ax12_4 (D1, D0, 4, 1000000);
00133         AX12 ax12_5 (D1, D0, 5, 1000000);*/
00134 
00135     /* Interrupt to start the robot */
00136     startup.fall(&go);
00137     /*motor1->Move(StepperMotor::BWD,6000);
00138     motor2->Move(StepperMotor::FWD,6000);
00139     motor1->WaitWhileActive();
00140     motor2->WaitWhileActive();
00141     motor1->HardStop();
00142     motor2->HardStop();*/
00143     //deplacement(10, 0);
00144     /*    rotation(90, 30);
00145         wait(2);
00146         rotation(45, 30);
00147         wait(2);
00148         rotation(180, 30);
00149         wait(2);
00150         rotation(45, 30);*/
00151     /*motor1->Move(StepperMotor::FWD,6000);
00152     motor2->Move(StepperMotor::BWD,6000);
00153     motor1->WaitWhileActive();
00154     motor2->WaitWhileActive();
00155     motor1->HardStop();
00156     motor2->HardStop();
00157     */
00158 
00159     while(start) {
00160         /* Waiting code */
00161         /*        ax12_1.SetSpeed(70);
00162                 ax12_2.SetSpeed(70);
00163                 ax12_3.SetSpeed(70);
00164                 ax12_4.SetSpeed(70);
00165                 ax12_5.SetSpeed(70);*/
00166         //wait_ms(100);
00167     }
00168 
00169     /* Interrupt to stop the robot */
00170     game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
00171 
00172     while(end) {
00173         //deplacement(60, 1);
00174         //motor1->Run(StepperMotor::FWD);
00175         //motor2->Run(StepperMotor::BWD);
00176         //wait(1);
00177         //motor1->Run(StepperMotor::BWD);
00178         //motor2->Run(StepperMotor::FWD);
00179         wait_ms(100);
00180         motor1->Move(StepperMotor::BWD,12000);
00181         motor2->Move(StepperMotor::FWD,12000);
00182         while((sensor.getDistance()>50) && end) {
00183             //Tant que la distance est superieure on continue d'avancer
00184             wait_ms(20);
00185             printf("1");
00186         }
00187     }
00188 
00189     parasol.write(0.5);
00190     printf("2");
00191     motor1->HardStop();
00192     motor2->HardStop();
00193 }
00194 
00195 void pwm()
00196 {
00197 }