Julien Tiron / Mbed 2 deprecated 2-FisherMan

de

Dependencies:   AX12 Servo VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed

main.cpp@0:88fd29503679, 2016-05-06 (annotated)

Committer:
julientiron
Date:
Fri May 06 07:23:13 2016 +0000
Revision:
0:88fd29503679
j;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julientiron 0:88fd29503679 1 /**
julientiron 0:88fd29503679 2 ******************************************************************************
julientiron 0:88fd29503679 3 * @file main.cpp
julientiron 0:88fd29503679 4 * @author Julien Tiron, FIP Télécom Bretagne
julientiron 0:88fd29503679 5 * @version V1.0.0
julientiron 0:88fd29503679 6 * @date March 23th, 2016
julientiron 0:88fd29503679 7 * @brief DoorCloser robot main code
julientiron 0:88fd29503679 8 ******************************************************************************
julientiron 0:88fd29503679 9 **/
julientiron 0:88fd29503679 10
julientiron 0:88fd29503679 11 /* Includes ------------------------------------------------------------------*/
julientiron 0:88fd29503679 12
julientiron 0:88fd29503679 13 #include "mbed.h"
julientiron 0:88fd29503679 14 #include "DevSPI.h"
julientiron 0:88fd29503679 15 #include "l6474_class.h"
julientiron 0:88fd29503679 16 #include "DevI2C.h"
julientiron 0:88fd29503679 17 #include "VL6180x.h"
julientiron 0:88fd29503679 18 #include "AX12.h"
julientiron 0:88fd29503679 19 #include "Servo.h"
julientiron 0:88fd29503679 20 /* Definitions ---------------------------------------------------------------*/
julientiron 0:88fd29503679 21
julientiron 0:88fd29503679 22 #define VL6180X_ADDRESS 0x29
julientiron 0:88fd29503679 23
julientiron 0:88fd29503679 24 /* Variables -----------------------------------------------------------------*/
julientiron 0:88fd29503679 25
julientiron 0:88fd29503679 26 /* Start and Stop Component */
julientiron 0:88fd29503679 27 InterruptIn startup(PC_1);
julientiron 0:88fd29503679 28 Ticker pwm_ticker;
julientiron 0:88fd29503679 29 Timeout game_length;
julientiron 0:88fd29503679 30 volatile bool start = 1;
julientiron 0:88fd29503679 31 volatile bool end = 1;
julientiron 0:88fd29503679 32 char data = 0x08 | (char)64;
julientiron 0:88fd29503679 33 bool tag = true;
julientiron 0:88fd29503679 34 int i2cAddres=0x70; // Address of DS1307 is 0x68 (7 bit address)
julientiron 0:88fd29503679 35 int i2c8BitAddres= i2cAddres <<1; // Convert to 8bit addressing used by mbed
julientiron 0:88fd29503679 36 Servo myservo(D5);
julientiron 0:88fd29503679 37 PwmOut parasol(A3);
julientiron 0:88fd29503679 38 /* Motor Control Component */
julientiron 0:88fd29503679 39 L6474 *motor1;
julientiron 0:88fd29503679 40 L6474 *motor2;
julientiron 0:88fd29503679 41 DigitalInOut sdaDummy(D14);
julientiron 0:88fd29503679 42 DigitalInOut sclDummy(D15);
julientiron 0:88fd29503679 43 /* Distance Sensors Component */
julientiron 0:88fd29503679 44 VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
julientiron 0:88fd29503679 45
julientiron 0:88fd29503679 46 /* Functions -----------------------------------------------------------------*/
julientiron 0:88fd29503679 47
julientiron 0:88fd29503679 48 void go()
julientiron 0:88fd29503679 49 {
julientiron 0:88fd29503679 50 start = 0;
julientiron 0:88fd29503679 51 }
julientiron 0:88fd29503679 52
julientiron 0:88fd29503679 53 void stop()
julientiron 0:88fd29503679 54 {
julientiron 0:88fd29503679 55 end = 0;
julientiron 0:88fd29503679 56 }
julientiron 0:88fd29503679 57
julientiron 0:88fd29503679 58 void init_sensor()
julientiron 0:88fd29503679 59 {
julientiron 0:88fd29503679 60
julientiron 0:88fd29503679 61 }
julientiron 0:88fd29503679 62
julientiron 0:88fd29503679 63 void switch_sensor(int number)
julientiron 0:88fd29503679 64 {
julientiron 0:88fd29503679 65
julientiron 0:88fd29503679 66 }
julientiron 0:88fd29503679 67
julientiron 0:88fd29503679 68 void pwm();
julientiron 0:88fd29503679 69
julientiron 0:88fd29503679 70 void deplacement(int distanceEnCm, bool sens)
julientiron 0:88fd29503679 71 {
julientiron 0:88fd29503679 72 int nbPas = distanceEnCm*166.7;
julientiron 0:88fd29503679 73 if(sens) {
julientiron 0:88fd29503679 74 motor1->Move(StepperMotor::BWD,nbPas);
julientiron 0:88fd29503679 75 motor2->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 76 } else {
julientiron 0:88fd29503679 77 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 78 motor2->Move(StepperMotor::BWD,nbPas);
julientiron 0:88fd29503679 79 }
julientiron 0:88fd29503679 80 motor1->WaitWhileActive();
julientiron 0:88fd29503679 81 motor2->WaitWhileActive();
julientiron 0:88fd29503679 82 motor1->HardStop();
julientiron 0:88fd29503679 83 motor2->HardStop();
julientiron 0:88fd29503679 84 }
julientiron 0:88fd29503679 85
julientiron 0:88fd29503679 86 void rotation(int angleEnDegre, int diametre)
julientiron 0:88fd29503679 87 {
julientiron 0:88fd29503679 88 int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
julientiron 0:88fd29503679 89 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 90 nbPas /= 990;
julientiron 0:88fd29503679 91
julientiron 0:88fd29503679 92 /*
julientiron 0:88fd29503679 93 int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
julientiron 0:88fd29503679 94 int nbPas = 160*deplacementEnCm;
julientiron 0:88fd29503679 95 nbPas *= 2;*/
julientiron 0:88fd29503679 96
julientiron 0:88fd29503679 97 printf("%d \n\r", nbPas);
julientiron 0:88fd29503679 98 motor2->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 99 motor1->Move(StepperMotor::FWD,nbPas);
julientiron 0:88fd29503679 100 motor1->WaitWhileActive();
julientiron 0:88fd29503679 101 motor2->WaitWhileActive();
julientiron 0:88fd29503679 102 motor1->HardStop();
julientiron 0:88fd29503679 103 motor2->HardStop();
julientiron 0:88fd29503679 104 }
julientiron 0:88fd29503679 105
julientiron 0:88fd29503679 106
julientiron 0:88fd29503679 107 /* Main ----------------------------------------------------------------------*/
julientiron 0:88fd29503679 108
julientiron 0:88fd29503679 109 int main()
julientiron 0:88fd29503679 110 {
julientiron 0:88fd29503679 111 /*----- Initialization. -----*/
julientiron 0:88fd29503679 112
julientiron 0:88fd29503679 113 /* Initializing SPI bus. */
julientiron 0:88fd29503679 114 DevSPI dev_spi(D11, D12, D13);
julientiron 0:88fd29503679 115 myservo.calibrate(0.00095, 90.0); // Calibrate the servo
julientiron 0:88fd29503679 116 parasol.period_ms(10);
julientiron 0:88fd29503679 117 parasol.pulsewidth_ms(1);
julientiron 0:88fd29503679 118 /* Initializing Motor Control Components. */
julientiron 0:88fd29503679 119 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
julientiron 0:88fd29503679 120 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
julientiron 0:88fd29503679 121 if (motor1->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 122 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 123 if (motor2->Init() != COMPONENT_OK)
julientiron 0:88fd29503679 124 exit(EXIT_FAILURE);
julientiron 0:88fd29503679 125 sdaDummy.mode(PullUp);
julientiron 0:88fd29503679 126 sclDummy.mode(PullUp);
julientiron 0:88fd29503679 127 //pwm_ticker.attach(&pwm, 0.1);
julientiron 0:88fd29503679 128 /* Initializing AX-12 motors */
julientiron 0:88fd29503679 129 /* AX12 ax12_1 (D1, D0, 1, 1000000);
julientiron 0:88fd29503679 130 AX12 ax12_2 (D1, D0, 2, 1000000);
julientiron 0:88fd29503679 131 AX12 ax12_3 (D1, D0, 3, 1000000);
julientiron 0:88fd29503679 132 AX12 ax12_4 (D1, D0, 4, 1000000);
julientiron 0:88fd29503679 133 AX12 ax12_5 (D1, D0, 5, 1000000);*/
julientiron 0:88fd29503679 134
julientiron 0:88fd29503679 135 /* Interrupt to start the robot */
julientiron 0:88fd29503679 136 startup.fall(&go);
julientiron 0:88fd29503679 137 /*motor1->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 138 motor2->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 139 motor1->WaitWhileActive();
julientiron 0:88fd29503679 140 motor2->WaitWhileActive();
julientiron 0:88fd29503679 141 motor1->HardStop();
julientiron 0:88fd29503679 142 motor2->HardStop();*/
julientiron 0:88fd29503679 143 //deplacement(10, 0);
julientiron 0:88fd29503679 144 /* rotation(90, 30);
julientiron 0:88fd29503679 145 wait(2);
julientiron 0:88fd29503679 146 rotation(45, 30);
julientiron 0:88fd29503679 147 wait(2);
julientiron 0:88fd29503679 148 rotation(180, 30);
julientiron 0:88fd29503679 149 wait(2);
julientiron 0:88fd29503679 150 rotation(45, 30);*/
julientiron 0:88fd29503679 151 /*motor1->Move(StepperMotor::FWD,6000);
julientiron 0:88fd29503679 152 motor2->Move(StepperMotor::BWD,6000);
julientiron 0:88fd29503679 153 motor1->WaitWhileActive();
julientiron 0:88fd29503679 154 motor2->WaitWhileActive();
julientiron 0:88fd29503679 155 motor1->HardStop();
julientiron 0:88fd29503679 156 motor2->HardStop();
julientiron 0:88fd29503679 157 */
julientiron 0:88fd29503679 158
julientiron 0:88fd29503679 159 while(start) {
julientiron 0:88fd29503679 160 /* Waiting code */
julientiron 0:88fd29503679 161 /* ax12_1.SetSpeed(70);
julientiron 0:88fd29503679 162 ax12_2.SetSpeed(70);
julientiron 0:88fd29503679 163 ax12_3.SetSpeed(70);
julientiron 0:88fd29503679 164 ax12_4.SetSpeed(70);
julientiron 0:88fd29503679 165 ax12_5.SetSpeed(70);*/
julientiron 0:88fd29503679 166 //wait_ms(100);
julientiron 0:88fd29503679 167 }
julientiron 0:88fd29503679 168
julientiron 0:88fd29503679 169 /* Interrupt to stop the robot */
julientiron 0:88fd29503679 170 game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
julientiron 0:88fd29503679 171
julientiron 0:88fd29503679 172 while(end) {
julientiron 0:88fd29503679 173 //deplacement(60, 1);
julientiron 0:88fd29503679 174 //motor1->Run(StepperMotor::FWD);
julientiron 0:88fd29503679 175 //motor2->Run(StepperMotor::BWD);
julientiron 0:88fd29503679 176 //wait(1);
julientiron 0:88fd29503679 177 //motor1->Run(StepperMotor::BWD);
julientiron 0:88fd29503679 178 //motor2->Run(StepperMotor::FWD);
julientiron 0:88fd29503679 179 wait_ms(100);
julientiron 0:88fd29503679 180 motor1->Move(StepperMotor::BWD,12000);
julientiron 0:88fd29503679 181 motor2->Move(StepperMotor::FWD,12000);
julientiron 0:88fd29503679 182 while((sensor.getDistance()>50) && end) {
julientiron 0:88fd29503679 183 //Tant que la distance est superieure on continue d'avancer
julientiron 0:88fd29503679 184 wait_ms(20);
julientiron 0:88fd29503679 185 printf("1");
julientiron 0:88fd29503679 186 }
julientiron 0:88fd29503679 187 }
julientiron 0:88fd29503679 188
julientiron 0:88fd29503679 189 parasol.write(0.5);
julientiron 0:88fd29503679 190 printf("2");
julientiron 0:88fd29503679 191 motor1->HardStop();
julientiron 0:88fd29503679 192 motor2->HardStop();
julientiron 0:88fd29503679 193 }
julientiron 0:88fd29503679 194
julientiron 0:88fd29503679 195 void pwm()
julientiron 0:88fd29503679 196 {
julientiron 0:88fd29503679 197 }