VRAC_HACKATHON / Mbed 2 deprecated FRC_2019_final

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@13:9b6578fa1669, 2019-06-08 (annotated)

Committer:
dagon
Date:
Sat Jun 08 08:51:07 2019 +0000
Revision:
13:9b6578fa1669
Parent:
12:09932ddcb089 
strat antoine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:4902b3f17137 1 #include "mbed.h"
haarkon 0:4902b3f17137 2 #include "CMPS03.h"
haarkon 0:4902b3f17137 3 #include "CNY70.h"
haarkon 0:4902b3f17137 4 #include "VMA306.h"
haarkon 3:0221daeeaa86 5 #include "Pixy.h"
haarkon 4:5038b4cd1088 6 #include "PID.h"
haarkon 4:5038b4cd1088 7
kyxstark 8:7b0fa8a914c0 8
haarkon 4:5038b4cd1088 9 #define PI 3.1415926535898
haarkon 0:4902b3f17137 10
duperoux_j 12:09932ddcb089 11 #define NSpeed 800
haarkon 7:6d58adc26b78 12
haarkon 3:0221daeeaa86 13 Serial pc (PA_2, PA_3, 921600);
kyxstark 8:7b0fa8a914c0 14 PID motor (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);
duperoux_j 12:09932ddcb089 15 VMA306 UltraSon (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6);
haarkon 0:4902b3f17137 16
duperoux_j 10:bb350e855c59 17 DigitalIn bp (PC_13);
duperoux_j 10:bb350e855c59 18 DigitalOut led1 (PA_5);
duperoux_j 10:bb350e855c59 19 DigitalOut led2 (PD_2);
duperoux_j 10:bb350e855c59 20
dagon 13:9b6578fa1669 21 Timer timer;
duperoux_j 10:bb350e855c59 22 double x, y, theta, vG, vD;
haarkon 0:4902b3f17137 23
haarkon 0:4902b3f17137 24
duperoux_j 10:bb350e855c59 25 //speed entre 0 et 1300
duperoux_j 10:bb350e855c59 26 void movement_rotate(int speed, int relative_angle_degree)
duperoux_j 10:bb350e855c59 27 {
duperoux_j 10:bb350e855c59 28 double destination_angle_degree = (relative_angle_degree * PI) / 180.0f;
duperoux_j 10:bb350e855c59 29 double starting_angle_rad = theta;
haarkon 0:4902b3f17137 30
duperoux_j 10:bb350e855c59 31 if (relative_angle_degree > 0.0f) {
duperoux_j 10:bb350e855c59 32 while (theta - starting_angle_rad < destination_angle_degree) {
duperoux_j 10:bb350e855c59 33 motor.setSpeed(-speed,speed);
duperoux_j 10:bb350e855c59 34 motor.getPosition (&x,&y, &theta);
duperoux_j 10:bb350e855c59 35 motor.getSpeed (&vG, &vD);
duperoux_j 10:bb350e855c59 36 }
duperoux_j 10:bb350e855c59 37 } else {
duperoux_j 10:bb350e855c59 38 while (theta - starting_angle_rad > destination_angle_degree) {
duperoux_j 10:bb350e855c59 39 motor.setSpeed(speed,-speed);
duperoux_j 10:bb350e855c59 40 motor.getPosition (&x,&y, &theta);
duperoux_j 10:bb350e855c59 41 motor.getSpeed (&vG, &vD);
duperoux_j 10:bb350e855c59 42 }
duperoux_j 10:bb350e855c59 43 }
duperoux_j 10:bb350e855c59 44 motor.setSpeed(0,0);
duperoux_j 10:bb350e855c59 45 }
haarkon 0:4902b3f17137 46
duperoux_j 10:bb350e855c59 47 //speed entre 0 et 1300
duperoux_j 10:bb350e855c59 48 void movement_linear(int speed, int relative_distance_mm)
duperoux_j 10:bb350e855c59 49 {
duperoux_j 10:bb350e855c59 50 double starting_traveled_distance = motor.getTraveledDistance_mm();
dagon 13:9b6578fa1669 51
duperoux_j 10:bb350e855c59 52 if (relative_distance_mm > 0.0f) {
duperoux_j 10:bb350e855c59 53 while (motor.getTraveledDistance_mm() - starting_traveled_distance < relative_distance_mm) {
duperoux_j 10:bb350e855c59 54 motor.setSpeed(speed,speed);
duperoux_j 10:bb350e855c59 55 motor.getPosition (&x,&y, &theta);
duperoux_j 10:bb350e855c59 56 motor.getSpeed (&vG, &vD);
duperoux_j 10:bb350e855c59 57 }
dagon 13:9b6578fa1669 58 } else {
duperoux_j 10:bb350e855c59 59 while (motor.getTraveledDistance_mm() - starting_traveled_distance > relative_distance_mm) {
duperoux_j 10:bb350e855c59 60 motor.setSpeed(-speed,-speed);
duperoux_j 10:bb350e855c59 61 motor.getPosition (&x,&y, &theta);
duperoux_j 10:bb350e855c59 62 motor.getSpeed (&vG, &vD);
duperoux_j 10:bb350e855c59 63 }
duperoux_j 10:bb350e855c59 64 }
haarkon 4:5038b4cd1088 65
duperoux_j 10:bb350e855c59 66 motor.setSpeed(0,0);
duperoux_j 10:bb350e855c59 67 }
duperoux_j 10:bb350e855c59 68
duperoux_j 10:bb350e855c59 69 //speed entre 0 et 1300
duperoux_j 10:bb350e855c59 70 //L'acceleration et la decelleration ne sont indispensab le qu'a partir des vitesses superieur a 300
duperoux_j 10:bb350e855c59 71 //calcul de la distance parcourue lors de l'acceleration: (speed/100)*acceleration_distance_mm [en mm]
dagon 13:9b6578fa1669 72 void movement_acceleration(int speed, int acceleration_distance_mm)
dagon 13:9b6578fa1669 73 {
duperoux_j 10:bb350e855c59 74 int acceleration_steps = speed / 100.0f;
duperoux_j 10:bb350e855c59 75 for (int i = 0; i < acceleration_steps ; i++) {
duperoux_j 10:bb350e855c59 76 movement_linear(100*(i+1), acceleration_distance_mm);
duperoux_j 10:bb350e855c59 77 }
duperoux_j 10:bb350e855c59 78 }
duperoux_j 10:bb350e855c59 79
duperoux_j 10:bb350e855c59 80 //speed entre 0 et 1300
dagon 13:9b6578fa1669 81 void movement_deceleration(int speed, int deceleration_distance_mm)
dagon 13:9b6578fa1669 82 {
duperoux_j 10:bb350e855c59 83 int acceleration_steps = speed / 100.0f;
duperoux_j 10:bb350e855c59 84 for (int i = acceleration_steps; i > 0 ; i--) {
duperoux_j 10:bb350e855c59 85 movement_linear(100*i, deceleration_distance_mm);
duperoux_j 10:bb350e855c59 86 }
duperoux_j 10:bb350e855c59 87 }
haarkon 0:4902b3f17137 88
dagon 13:9b6578fa1669 89 void end_game()
dagon 13:9b6578fa1669 90 {
dagon 13:9b6578fa1669 91 motor.setSpeed(0, 0);
dagon 13:9b6578fa1669 92 while (1) {
dagon 13:9b6578fa1669 93 led1 = 1;
dagon 13:9b6578fa1669 94 led2 = 0;
dagon 13:9b6578fa1669 95 wait(0.3);
dagon 13:9b6578fa1669 96 led1 = 0;
dagon 13:9b6578fa1669 97 led2 = 1;
dagon 13:9b6578fa1669 98 wait(0.3);
dagon 13:9b6578fa1669 99 }
dagon 13:9b6578fa1669 100 }
dagon 13:9b6578fa1669 101
dagon 13:9b6578fa1669 102 void automate_evitemment(int speed, int seuil_face, int seuil_cote)
dagon 13:9b6578fa1669 103 {
duperoux_j 12:09932ddcb089 104 motor.setSpeed(speed, speed);
dagon 13:9b6578fa1669 105
duperoux_j 12:09932ddcb089 106 double val_us_gauche = 0, val_us_face = 0, val_us_droite = 0;
duperoux_j 12:09932ddcb089 107 while (1) {
dagon 13:9b6578fa1669 108 if (timer.read() > 80.0f)
dagon 13:9b6578fa1669 109 end_game();
dagon 13:9b6578fa1669 110
duperoux_j 12:09932ddcb089 111 if (UltraSon.isUSGReady()) val_us_gauche = UltraSon.readUSG();
duperoux_j 12:09932ddcb089 112 if (UltraSon.isUSBReady()) val_us_face = UltraSon.readUSB();
duperoux_j 12:09932ddcb089 113 if (UltraSon.isUSDReady()) val_us_droite = UltraSon.readUSD();
dagon 13:9b6578fa1669 114
duperoux_j 12:09932ddcb089 115 if (val_us_face < seuil_face) {
duperoux_j 12:09932ddcb089 116 movement_rotate(speed, 80);
dagon 13:9b6578fa1669 117 } else if (val_us_gauche < seuil_cote) {
duperoux_j 12:09932ddcb089 118 movement_rotate(speed, -40);
dagon 13:9b6578fa1669 119 } else if (val_us_droite < seuil_cote) {
duperoux_j 12:09932ddcb089 120 movement_rotate(speed, 40);
duperoux_j 12:09932ddcb089 121 }
dagon 13:9b6578fa1669 122
duperoux_j 12:09932ddcb089 123 motor.setSpeed(speed, speed);
duperoux_j 12:09932ddcb089 124 wait(0.005);
duperoux_j 12:09932ddcb089 125 }
duperoux_j 12:09932ddcb089 126 }
duperoux_j 12:09932ddcb089 127
dagon 13:9b6578fa1669 128
dagon 13:9b6578fa1669 129
dagon 13:9b6578fa1669 130
haarkon 3:0221daeeaa86 131 main ()
haarkon 3:0221daeeaa86 132 {
dagon 13:9b6578fa1669 133 timer.start();
dagon 13:9b6578fa1669 134
kyxstark 8:7b0fa8a914c0 135 motor.resetPosition();
duperoux_j 12:09932ddcb089 136
duperoux_j 12:09932ddcb089 137
dagon 13:9b6578fa1669 138 /*movement_acceleration(1000, 100);
dagon 13:9b6578fa1669 139 movement_linear(1000, 1850 - 100*10 - 100*10); //bas
dagon 13:9b6578fa1669 140 movement_deceleration(1000, 100);*/
duperoux_j 12:09932ddcb089 141
dagon 13:9b6578fa1669 142 movement_linear(300, 1850); //bas
dagon 13:9b6578fa1669 143 wait(0.5);
dagon 13:9b6578fa1669 144
dagon 13:9b6578fa1669 145 movement_rotate(200, -90);
dagon 13:9b6578fa1669 146 wait(0.5);
dagon 13:9b6578fa1669 147
dagon 13:9b6578fa1669 148 movement_linear(300, 2275); //mid
dagon 13:9b6578fa1669 149 wait(0.5);
dagon 13:9b6578fa1669 150
dagon 13:9b6578fa1669 151 /*movement_rotate(100, -47.5);
dagon 13:9b6578fa1669 152 wait(0.5);
dagon 13:9b6578fa1669 153
dagon 13:9b6578fa1669 154 movement_linear(300, 2950); //mid
dagon 13:9b6578fa1669 155 wait(0.5);*/
dagon 13:9b6578fa1669 156
dagon 13:9b6578fa1669 157 movement_rotate(100, -30);
dagon 13:9b6578fa1669 158 wait(0.5);
dagon 13:9b6578fa1669 159
dagon 13:9b6578fa1669 160 movement_linear(300, 75); // riz
dagon 13:9b6578fa1669 161 wait(0.25);
dagon 13:9b6578fa1669 162
dagon 13:9b6578fa1669 163 movement_rotate(100, 60);
dagon 13:9b6578fa1669 164 wait(0.25);
dagon 13:9b6578fa1669 165
dagon 13:9b6578fa1669 166 movement_rotate(100, -60);
dagon 13:9b6578fa1669 167 wait(0.25);
dagon 13:9b6578fa1669 168
dagon 13:9b6578fa1669 169 movement_linear(300, 75); // riz
dagon 13:9b6578fa1669 170 wait(0.25);
dagon 13:9b6578fa1669 171
dagon 13:9b6578fa1669 172 movement_rotate(100, 60);
dagon 13:9b6578fa1669 173 wait(0.25);
dagon 13:9b6578fa1669 174
dagon 13:9b6578fa1669 175 movement_rotate(100, -60);
dagon 13:9b6578fa1669 176 wait(0.25);
dagon 13:9b6578fa1669 177
dagon 13:9b6578fa1669 178 movement_linear(300, 75); // riz
dagon 13:9b6578fa1669 179 wait(0.25);
dagon 13:9b6578fa1669 180
dagon 13:9b6578fa1669 181 movement_rotate(100, 60);
dagon 13:9b6578fa1669 182 wait(0.25);
dagon 13:9b6578fa1669 183
dagon 13:9b6578fa1669 184 movement_rotate(100, -60);
dagon 13:9b6578fa1669 185 wait(0.25);
dagon 13:9b6578fa1669 186
dagon 13:9b6578fa1669 187 movement_linear(300, 75); // riz
dagon 13:9b6578fa1669 188 wait(0.25);
dagon 13:9b6578fa1669 189
dagon 13:9b6578fa1669 190 movement_rotate(100, 60);
dagon 13:9b6578fa1669 191 wait(0.25);
dagon 13:9b6578fa1669 192
dagon 13:9b6578fa1669 193 movement_rotate(100, -60);
dagon 13:9b6578fa1669 194 wait(0.25);
dagon 13:9b6578fa1669 195
dagon 13:9b6578fa1669 196 movement_linear(300, 75); // riz
dagon 13:9b6578fa1669 197 wait(0.25);
dagon 13:9b6578fa1669 198
dagon 13:9b6578fa1669 199 movement_rotate(100, 60);
dagon 13:9b6578fa1669 200 wait(0.25);
dagon 13:9b6578fa1669 201
dagon 13:9b6578fa1669 202 movement_rotate(100, -30);
dagon 13:9b6578fa1669 203 wait(0.25);
dagon 13:9b6578fa1669 204
dagon 13:9b6578fa1669 205 automate_evitemment(400, 50, 40);
dagon 13:9b6578fa1669 206
dagon 13:9b6578fa1669 207 while (1);
dagon 13:9b6578fa1669 208
haarkon 0:4902b3f17137 209 }