Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 813f014e3d1f1a42fbd0851c3d877e1739d8710a
|
From: Justin <Ba...@us...> - 2011-06-09 00:29:34
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 813f014e3d1f1a42fbd0851c3d877e1739d8710a (commit)
from f36c674021c358ca888dbe3288de075e51f86556 (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 813f014e3d1f1a42fbd0851c3d877e1739d8710a
Author: justin <justin@MacBook.(none)>
Date: Thu Jun 9 02:28:42 2011 +0200
add rotation during the movment
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#emission.c# b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#emission.c#
deleted file mode 100644
index e41799f..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#emission.c#
+++ /dev/null
@@ -1 +0,0 @@
-STM32STM32
\ No newline at end of file
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#main.c# b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#main.c#
deleted file mode 100644
index 77b78d4..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/#main.c#
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Firmware for Controller Motor STM32
- *
- * Author : Xavier Lagorce <Xav...@cr...>
- */
-#include <cpu/irq.h>
-#include <cfg/debug.h>
-#include <drv/timer.h>
-#include <drv/gpio_stm32.h>
-#include <kern/proc.h>
-#include <kern/monitor.h>
-#include <math.h>
-
-#include "hw/hw_led.h"
-#include "motor.h"
-#include "encoder.h"
-#include "asservissement.h"
-
-
-PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
-
-static void init(void)
-{
- IRQ_ENABLE;
- LEDS_INIT();
-
- /* Initialize debugging module (allow kprintf(), etc.) */
- kdbg_init();
- /* Initialize system timer */
- timer_init();
-
- /*
- * Kernel initialization: processes (allow to create and dispatch
- * processes using proc_new()).
- */
- proc_init();
-
- motorsInit();
- encodersInit();
- controlInit();
-
- // Blink to say we are ready
- for (uint8_t i=0; i < 5; i++) {
- LED1_ON();
- LED2_ON();
- LED3_ON();
- LED4_ON();
- timer_delay(100);
- LED1_OFF();
- LED2_OFF();
- LED3_OFF();
- LED4_OFF();
- timer_delay(100);
- }
-}
-
-static void NORETURN ind_process(void)
-{
- while(1) {
- LED1_ON();
- timer_delay(500);
- LED1_OFF();
- timer_delay(500);
- }
-}
-
-int main(void)
-{
- init();
- int status=0;
- int coder=0;
- /* Create a new child process */
- proc_new(ind_process, NULL, sizeof(stack_ind), stack_ind);
-
- /*
- * The main process is kept to periodically report the stack
- * utilization of all the processes (1 probe per second).
- */
-
-
-
-
- while (1)
- {
- //monitor_report();
- /* if( getEncoderDirection(ENCODER3) == FORWARD_DIRECTION )
- {
- if(status==0)
- {
- status=1;
- LED4_ON();
- }
- }
- else
- {
- if(status==1)
- {
- LED4_OFF();
- status=0;
- }
- LED4_OFF();
- }
- coder = getEncoderCount(ENCODER2);
- timer_delay(10);
- if ( getEncoderCount(ENCODER2)==65535) LED3_ON();
- //else LED3_OFF();
- timer_delay(10);*/
- timer_delay(1000);
- }
-
- return 0;
-}
-
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/reception.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/reception.c
index 9f36a29..8990b3a 100644
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/reception.c
+++ b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/reception.c
@@ -182,7 +182,7 @@ uint16_t calculate_weight(uint16_t k){
if((abs(k-i) < 5) & (i!=k)){
- weight += detectionState[i]*pow(3,5-abs(k-i));
+ weight += detectionState[i]*((uint16_t)pow(3,5-abs(k-i)));
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
index e122cd0..5471c5c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
@@ -435,6 +435,7 @@ void generateRampeRobotPlan() {
//Def des variables nécessaires à la générations des rampes
float xmax, ymax,vmax,a;
float P,V;
+ float W, thetaMax, vthetaMax,aW;
float xVirtuel, yVirtuel;
float Vx, Vy, theta;
float Pvmax,tvmax,tmax;
@@ -442,7 +443,8 @@ void generateRampeRobotPlan() {
float t; //Défini le temps discret
int N=0; //Nombre de points dans le vecteur de rampe
float t1, t2; //Temps pour les trapèzes
- float desiredP;
+ float desiredP, desiredT;
+ int signeT=1; //suivant la valeur de DTheta = variation de l'angle du robot
float Xstart, Ystart, thetaRobot, Tstart;
float Ti[3]={0,0,0};
float w[3]={0,0,0};
@@ -486,14 +488,26 @@ void generateRampeRobotPlan() {
xmax = holonome.xmax;
ymax = holonome.ymax;
vmax = holonome.vitesse;
+
+ thetaMax = holonome.Tmax;
+ vthetaMax = holonome.vTmax;
+
a = holonome.acceleration;
Xstart = holonome.Xstart;
Ystart = holonome.Ystart;
- theta = holonome.theta;
- thetaRobot = holonome_odometry.T;
+ Tstart = holonome.Tstart;
+ theta = holonome.theta; //theta de la droite dans le repère de la table
+ thetaRobot = holonome_odometry.T; //theta du robot dans le repère de la table
desiredP=sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart));
-
+ desiredT=thetaMax - Tstart;
+
+ if(desiredT<0) {
+ signeT=-1;
+ desiredT = -desiredT;
+ }
+
+ //Calcul de la vitesse linéaire
//Choix du profil : triangulaire ou trapézoïdal
tvmax = vmax/a;
Pvmax = a*tvmax*tvmax;
@@ -566,7 +580,58 @@ void generateRampeRobotPlan() {
Vx = V*cos(theta-thetaRobot);
Vy = V*sin(theta-thetaRobot);
-
+
+ //Calcul de la composante de rotation
+ //Choix du profil : triangulaire ou trapézoïdal
+
+ if(vthetaMax*tmax/2>desiredT) {
+ //On génère un profil triangulaire
+ aW = desiredT*4/(tmax*tmax);
+ if(t<=tmax/2) {
+ //On est dans la première partie de la courbe
+ //Calcul de aW : accélération sur theta
+ W=aW*t;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la deuxième partie de la coube
+ W=-aW*t+aW*tmax;
+ }
+ else {
+ W=0;
+ }
+ }
+ }
+ else {
+ //On génère un profil trapézoïdal
+ t1=tmax-desiredT/vthetaMax;
+ t2 = tmax - t1;
+
+ t=k*T;
+ aW = vthetaMax/t1;
+ if(t<=t1) {
+ //On est dans la première partie de la courbe
+ W=aW*t;
+ }
+ else {
+ if(t<=t2) {
+ // On est dans la deuxième partie de la courbe
+ W=vthetaMax;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la troisième partie de la coube
+ W=-aW*t+aW*tmax;
+ }
+ else {
+ W=0;
+ }
+ }
+
+ }
+ }
+
+ W = signeT*W;
//Ajout des composantes de déviations
xVirtuel = cos(theta)*holonome.Pprevious;
@@ -576,9 +641,9 @@ void generateRampeRobotPlan() {
//Vx += ((xVirtuel+Xstart-holonome_odometry.X)*cos(thetaRobot)-(yVirtuel+Ystart-holonome_odometry.Y)*sin(thetaRobot))/(5*T);
//Vy += ((yVirtuel+Ystart-holonome_odometry.Y)*cos(thetaRobot)+(xVirtuel+Xstart-holonome_odometry.X)*sin(thetaRobot))/(5*T);
- w[0] = (-Vy)/R;
- w[1] = (Vx*sqrt(3)/2+Vy/2)/R;
- w[2] = (-Vx*sqrt(3)/2+Vy/2)/R;
+ w[0] = (-Vy)/R-W*L1/R;
+ w[1] = (Vx*sqrt(3)/2+Vy/2)/R-W*L2/R;
+ w[2] = (-Vx*sqrt(3)/2+Vy/2)/R-W*L3/R;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
index eb6678f..bc3efa8 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
@@ -14,7 +14,7 @@
PROC_DEFINE_STACK(stack_avoid, KERN_MINSTACKSIZE * 4);
int actual_state;
-float xmax, ymax;
+float xmax, ymax, Tmax;
int num_wait;
unsigned short stop_yet;
@@ -125,7 +125,7 @@ void make_choice(void) {
for(i=0;i<NUM_SENSORS;i++) {
if(sharp_sensors.state_trigger[i]==1) {
//Trigger is ON
- angle = reset_M_PI((sharp_sensors.circle_position[i]-holonome_odometry.T)-holonome.theta);
+ angle = reset_M_PI((sharp_sensors.circle_position[i]+holonome_odometry.T)-holonome.theta);
if(abs(angle)<DETECTION_AREA/2 || abs((sharp_sensors.circle_position[i]-holonome_odometry.T)-holonome.theta-0*M_PI)<DETECTION_AREA/2) {
result=1;
//LED2_ON();
@@ -139,10 +139,13 @@ void make_choice(void) {
holonome.asserPosition=0;
xmax = holonome.xmax;
ymax = holonome.ymax;
+ Tmax = holonome.Tmax;
holonome.X = holonome_odometry.X;
holonome.Y = holonome_odometry.Y;
+ holonome.T = holonome_odometry.T;
holonome.xmax = holonome_odometry.X;
holonome.ymax = holonome_odometry.Y;
+ holonome.Tmax = holonome_odometry.T;
if(num_wait>=0) stop_yet=1;
num_wait++;
obstacle=1;
@@ -153,8 +156,10 @@ void make_choice(void) {
holonome.asserPosition = 0;
holonome.X = holonome_odometry.X;
holonome.Y = holonome_odometry.Y;
+ holonome.T = holonome_odometry.T;
holonome.xmax = xmax;
holonome.ymax = ymax;
+ holonome.Tmax = Tmax;
stop_yet=0;
num_wait=0;
obstacle=0;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
index b75c2ac..f3ad693 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
@@ -29,8 +29,8 @@ void intelligenceInit(void) {
holonome.ymax=-0;
holonome.vYmax=0.3;
holonome.ay=0.15;
- holonome.Tmax=20*M_PI;
- holonome.vTmax=3;
+ holonome.Tmax=M_PI;
+ holonome.vTmax=5;
holonome.aT=0.5;
holonome.asserPosition=0;
holonome.X=0;
@@ -81,6 +81,7 @@ void makePath(void) {
holonome.X = holonome_odometry.X;
holonome.Y = holonome_odometry.Y;
+ holonome.T = holonome_odometry.T;
//holonome.X = holonome.xmax;
//holonome.Y = holonome.ymax;
@@ -91,14 +92,14 @@ void makePath(void) {
switch(etape) {
case 0://avance droit A(1,2)
- ouvrir_pinces();
+ //ouvrir_pinces();
holonome.busy=1;
holonome.asserPosition=0;
- holonome.xmax = -0.5;
+ holonome.xmax = -2.5;
holonome.ymax = signe*0;
- holonome.vitesse = 0.5;
+ holonome.vitesse = 0.25;
holonome.acceleration=0.5;
- ouvrir_pinces();
+ //ouvrir_pinces();
etape+=1;
break;
case 1://recul
@@ -106,12 +107,13 @@ void makePath(void) {
holonome.asserPosition=0;
holonome.xmax = -0.45;
holonome.ymax = signe*0;
- holonome.vitesse = 0.5;
+ holonome.vitesse = 0.25;
holonome.acceleration=0.5;
+ holonome.Tmax = 0;
//ouvrir_pinces();
etape+=1;
break;
- case 2://drift sur la droite
+ /*case 2://drift sur la droite
holonome.busy=1;
holonome.asserPosition=0;
holonome.xmax = -0.45;
@@ -237,7 +239,7 @@ void makePath(void) {
holonome.acceleration=0.5;
etape+=1;
break;
- /*case 15:
+ case 15:
holonome.busy=1;
holonome.asserPosition=0;
holonome.xmax = -0.75;
hooks/post-receive
--
krobot
|
