Diff: main.cpp

Revision:

0:88fd29503679

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 06 07:23:13 2016 +0000
@@ -0,0 +1,197 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  Julien Tiron, FIP Télécom Bretagne
+ * @version V1.0.0
+ * @date    March 23th, 2016
+ * @brief   DoorCloser robot main code
+ ******************************************************************************
+ **/
+
+/* Includes ------------------------------------------------------------------*/
+
+#include "mbed.h"
+#include "DevSPI.h"
+#include "l6474_class.h"
+#include "DevI2C.h"
+#include "VL6180x.h"
+#include "AX12.h"
+#include "Servo.h"
+/* Definitions ---------------------------------------------------------------*/
+
+#define VL6180X_ADDRESS 0x29
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Start and  Stop Component */
+InterruptIn startup(PC_1);
+Ticker pwm_ticker;
+Timeout game_length;
+volatile bool start = 1;
+volatile bool end = 1;
+char data = 0x08 | (char)64;
+bool tag = true;
+int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
+int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
+Servo myservo(D5);
+PwmOut parasol(A3);
+/* Motor Control Component */
+L6474 *motor1;
+L6474 *motor2;
+DigitalInOut sdaDummy(D14);
+DigitalInOut sclDummy(D15);
+/* Distance Sensors Component */
+VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
+
+/* Functions -----------------------------------------------------------------*/
+
+void go()
+{
+    start = 0;
+}
+
+void stop()
+{
+    end = 0;
+}
+
+void init_sensor()
+{
+
+}
+
+void switch_sensor(int number)
+{
+
+}
+
+void pwm();
+
+void deplacement(int distanceEnCm, bool sens)
+{
+    int nbPas = distanceEnCm*166.7;
+    if(sens) {
+        motor1->Move(StepperMotor::BWD,nbPas);
+        motor2->Move(StepperMotor::FWD,nbPas);
+    } else {
+        motor1->Move(StepperMotor::FWD,nbPas);
+        motor2->Move(StepperMotor::BWD,nbPas);
+    }
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
+}
+
+void rotation(int angleEnDegre, int diametre)
+{
+    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas /= 990;
+
+    /*
+    int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas *= 2;*/
+
+    printf("%d \n\r", nbPas);
+    motor2->Move(StepperMotor::FWD,nbPas);
+    motor1->Move(StepperMotor::FWD,nbPas);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
+}
+
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+    /*----- Initialization. -----*/
+
+    /* Initializing SPI bus. */
+    DevSPI dev_spi(D11, D12, D13);
+    myservo.calibrate(0.00095, 90.0); // Calibrate the servo
+    parasol.period_ms(10);
+    parasol.pulsewidth_ms(1);
+    /* Initializing Motor Control Components. */
+    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
+    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
+    if (motor1->Init() != COMPONENT_OK)
+        exit(EXIT_FAILURE);
+    if (motor2->Init() != COMPONENT_OK)
+        exit(EXIT_FAILURE);
+    sdaDummy.mode(PullUp);
+    sclDummy.mode(PullUp);
+    //pwm_ticker.attach(&pwm, 0.1);
+    /* Initializing AX-12 motors */
+    /*    AX12 ax12_1 (D1, D0, 1, 1000000);
+        AX12 ax12_2 (D1, D0, 2, 1000000);
+        AX12 ax12_3 (D1, D0, 3, 1000000);
+        AX12 ax12_4 (D1, D0, 4, 1000000);
+        AX12 ax12_5 (D1, D0, 5, 1000000);*/
+
+    /* Interrupt to start the robot */
+    startup.fall(&go);
+    /*motor1->Move(StepperMotor::BWD,6000);
+    motor2->Move(StepperMotor::FWD,6000);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();*/
+    //deplacement(10, 0);
+    /*    rotation(90, 30);
+        wait(2);
+        rotation(45, 30);
+        wait(2);
+        rotation(180, 30);
+        wait(2);
+        rotation(45, 30);*/
+    /*motor1->Move(StepperMotor::FWD,6000);
+    motor2->Move(StepperMotor::BWD,6000);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
+    */
+
+    while(start) {
+        /* Waiting code */
+        /*        ax12_1.SetSpeed(70);
+                ax12_2.SetSpeed(70);
+                ax12_3.SetSpeed(70);
+                ax12_4.SetSpeed(70);
+                ax12_5.SetSpeed(70);*/
+        //wait_ms(100);
+    }
+
+    /* Interrupt to stop the robot */
+    game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
+
+    while(end) {
+        //deplacement(60, 1);
+        //motor1->Run(StepperMotor::FWD);
+        //motor2->Run(StepperMotor::BWD);
+        //wait(1);
+        //motor1->Run(StepperMotor::BWD);
+        //motor2->Run(StepperMotor::FWD);
+        wait_ms(100);
+        motor1->Move(StepperMotor::BWD,12000);
+        motor2->Move(StepperMotor::FWD,12000);
+        while((sensor.getDistance()>50) && end) {
+            //Tant que la distance est superieure on continue d'avancer
+            wait_ms(20);
+            printf("1");
+        }
+    }
+
+    parasol.write(0.5);
+    printf("2");
+    motor1->HardStop();
+    motor2->HardStop();
+}
+
+void pwm()
+{
+}
\ No newline at end of file