ARES
Time is good
Fork of
Robot2016_2-0
by 
ARES
Revision 39:309f38d1e49c, committed 2016-04-05
- Comitter:
- IceTeam
- Date:
- Tue Apr 05 15:02:12 2016 +0000
- Parent:
- 37:da3a2c781672
- Child:
- 41:b5a2fbc20beb
- Commit message:
- Petite sauvegarde de batard;
Changed in this revision
--- a/Map/Map.cpp Sat Feb 06 10:11:28 2016 +0000
+++ b/Map/Map.cpp Tue Apr 05 15:02:12 2016 +0000
@@ -68,9 +68,9 @@
obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15*/
- obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,700,1000,30));// P16
- obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,30));// P16
- obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,900,800,50));// P16
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,700,1000,100));// P16
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,100));// P16
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,1000,100));// P16
}
int Map::getHeight(float x, float y)
--- a/Map/defines.h Sat Feb 06 10:11:28 2016 +0000
+++ b/Map/defines.h Tue Apr 05 15:02:12 2016 +0000
@@ -48,7 +48,7 @@
IDO_DEPOT_P
};
-#define ROBOTRADIUS 190
+#define ROBOTRADIUS 150
#define MAXPOINT 8000
--- a/Odometry/Odometry.cpp Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.cpp Tue Apr 05 15:02:12 2016 +0000
@@ -193,3 +193,18 @@
while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
wait(0.4);
}
+
+void Odometry::Forward(float i) {
+ int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
+
+ int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
+ int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
+
+ roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+
+ //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+
+ while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+ wait(0.4);
+ led = 1;
+}
\ No newline at end of file
--- a/Odometry/Odometry.h Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.h Tue Apr 05 15:02:12 2016 +0000
@@ -9,12 +9,16 @@
/* Vitesse d'acceleration d'angle reduite de 8000->4000 */
#define accel_angle 8000
-#define vitesse_angle 16000
+#define vitesse_angle 12000
#define deccel_angle 8000
-#define accel_dista 8000
-#define vitesse_dista 16000
-#define deccel_dista 8000
+#define accel_dista 12000
+#define vitesse_dista 20000
+#define deccel_dista 12000
+
+/* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
+#define ENTRAXE 243.8
+
/*
* Author : Benjamin Bertelone, reworked by Simon Emarre
@@ -36,8 +40,12 @@
*/
Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc);
+ /** Demande au robot d'effectuer un certain nombre de tour sur lui même */
void TestEntraxe(int i);
+ /** Demande au robot d'effectuer un déplacement sur l'avant. Voir si l'on peut enlever la correction PID */
+ void Forward(float i);
+
/** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
*/
void setPos(double x, double y, double theta);
--- a/RoboClaw.lib Sat Feb 06 10:11:28 2016 +0000 +++ b/RoboClaw.lib Tue Apr 05 15:02:12 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/ARES/code/RoboClaw/#ad00b3431ff5 +https://developer.mbed.org/teams/ARES/code/RoboClaw/#ece8a8cdf4b0
--- a/main.cpp Sat Feb 06 10:11:28 2016 +0000
+++ b/main.cpp Tue Apr 05 15:02:12 2016 +0000
@@ -5,7 +5,6 @@
#define ATTENTE 0
#define GO 1
#define STOP 2
-#define ENTRAXE 248.25
InterruptIn mybutton(USER_BUTTON);
DigitalIn button(USER_BUTTON);
@@ -14,7 +13,7 @@
//Serial pc(USBTX, USBRX);
Serial logger (PA_9, PA_10);
RoboClaw roboclaw(460800, PA_11, PA_12);
-Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
+Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
int i = 0;
@@ -27,7 +26,9 @@
int main(void)
{
init();
- /*double angle_v = 2*PI/5;
+ /* Code AStar */
+
+ double angle_v = 2*PI/5;
double distance_v = 200.0;
std::vector<SimplePoint> path;
Map map;
@@ -40,7 +41,7 @@
float y=odo.getY();
float the = 0;
- map.AStar(x, y, 1400, 1000, 75);
+ map.AStar(x, y, 1600, 1000, 75);
path = map.path;
for(int i=0; i<path.size();i++) {
@@ -62,37 +63,75 @@
//odo.GotoThet(-PI/2);
wait(2000);
- //odo.GotoXYT(2250,500,0);*/
+ //odo.GotoXYT(2250,500,0);
+
+ //odo.TestEntraxe(5);
+ //odo.Forward(1000);
+
+ /* Code JPO :
+ roboclaw.ForwardM1(ADR, 0);
+ roboclaw.ForwardM2(ADR, 0);
+ int state = 0;
while(1)
{
- while(logger.readable())
- {
+ // while(logger.readable())
+ //{
char c = logger.getc();
if(c=='z')
{
- roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, speed_angle);
+ if (state != 1) {
+ state = 1;
+ logger.printf("Avant (Z) \r\n");
+ roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+ roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+ }
}
else if(c == 's')
{
- roboclaw.ForwardM1(ADR, 0);
- roboclaw.ForwardM2(ADR, 0);
+ if (state != 2) {
+ state = 2;
+ logger.printf("Stop (S) \r\n");
+ roboclaw.ForwardM1(ADR, 0);
+ roboclaw.ForwardM2(ADR, 0);
+ }
}
else if(c == 'd')
{
- roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, speed_angle);
+ if (state != 3) {
+ state = 3;
+ logger.printf("Droite (D) \r\n");
+ roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+ roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+ }
}
else if(c == 'q')
{
- roboclaw.SpeedAccelM1M2(ADR, accel_angle, speed_angle, accel_angle, -speed_angle);
+ if (state != 4) {
+ state = 4;
+ logger.printf("Gauche (Q)\r\n");
+ roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+ roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+ }
}
else if(c == 'x')
{
- roboclaw.SpeedAccelM1M2(ADR, accel_angle, -speed_angle, accel_angle, -speed_angle);
+ if (state != 5) {
+ state = 5;
+ roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
+ }
}
- }
- roboclaw.ForwardM1(ADR, 0);
- roboclaw.ForwardM2(ADR, 0);
- }
+ else if (c == '\0') { ; }
+ else {
+ if (state != 0) {
+ roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
+ state = 0;
+ }
+ }
+ // }
+ // roboclaw.ForwardM1(ADR, 0);
+ // roboclaw.ForwardM2(ADR, 0);
+
+ }*/
}
void init(void)
@@ -108,9 +147,10 @@
while(button);
wait(1);
mybutton.fall(&pressed);
- mybutton.rise(&unpr
- essed);
+ mybutton.rise(&unpressed);
ticker.attach_us(&update_main,dt); // 100 Hz
+
+ logger.printf("End init\n\r");
}
void update_main(void)