Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. b9299bda41b472ccae373271bac0ca570cfece43
|
From: Justin <Ba...@us...> - 2011-06-08 14:43:06
|
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 b9299bda41b472ccae373271bac0ca570cfece43 (commit)
from e2f20d045bee49b75db46177cf3c28103bfee276 (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 b9299bda41b472ccae373271bac0ca570cfece43
Author: justin <justin@MacBook.(none)>
Date: Wed Jun 8 16:34:23 2011 +0200
Delete some files
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.c
deleted file mode 100644
index f760aea..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.c
+++ /dev/null
@@ -1,765 +0,0 @@
-#include "asservissement.h"
-#include "encoder.h"
-#include "motor.h"
-#include "odometry.h"
-
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x.h"
-#include <math.h>
-
-
-
-
-
-
-unsigned short def_encoders[4]={ENCODER1, ENCODER2, ENCODER3, ENCODER4};
-float Kp[NUM_MOTORS]={KP1,KP2,KP3,KP4};
-
-PROC_DEFINE_STACK(stack_control, KERN_MINSTACKSIZE * 4);
-
-
-void controlInit(void) {
- int i;
-
-
- for(i=0;i<NUM_ENCODERS;i++)
- {
- encoders_status[i].globalCounter=0;
- encoders_status[i].direction=0;
- encoders_status[i].speed=0;
- encoders_status[i].rampePosition=0;
- encoders_status[i].desiredPosition=0;
- encoders_status[i].startPosition=0;
- encoders_status[i].previousCounter=getEncoderCount(1<<i);
- encoders_status[i].busy=0;
- }
- /* Create a new child process */
-
- proc_new(control_process, NULL, sizeof(stack_control), stack_control);
-
-
- return;
-}
-
-void NORETURN control_process(void)
-{
- Timer timer_control_process;
- int64_t desiredImpulsions[NUM_MOTORS];
- int i;
- float desiredPositions[NUM_MOTORS]={0,2*M_PI,2*M_PI,2*M_PI};
- timer_setDelay(&timer_control_process, us_to_ticks((utime_t)(control_refresh)));
- timer_setEvent(&timer_control_process);
-
- enableMotors( MOTEUR4|MOTEUR2|MOTEUR3);
- //convertRadInImpulsions(desiredPositions, desiredImpulsions);
-
-
- while(1) {
-
- timer_add(&timer_control_process);// Start process timer
- /*for(i=0;i<NUM_MOTORS;i++)
- {
- desiredPositions[i]+=0.05;
- if (desiredPositions[i]>(2*M_PI)) desiredPositions[i]=2*M_PI;
- }
- convertRadInImpulsions(desiredPositions, desiredImpulsions);*/
- //generateRampeMotor(MOTEUR4|MOTEUR2|MOTEUR3,-20*2*M_PI,10,5);
- generateRampeRobotPlan();
- controlMotors(MOTEUR4|MOTEUR2|MOTEUR3);
-
- timer_waitEvent(&timer_control_process); // Wait until the end of counting
- }
-
-
-}
-
-void refreshEncoderStatus(unsigned short encoder) {
- int i;
- int64_t difference;
- int32_t currentCounter;
- for(i=0;i<NUM_ENCODERS;i++) {
- if(encoder & (1<<i)) {
- /* Def direction*/
- encoders_status[i].direction = getEncoderDirection(def_encoders[i]);
- /* Calcul du compteur global*/
- currentCounter =(int32_t)(getEncoderCount(def_encoders[i]));
- difference = (int64_t)(currentCounter-encoders_status[i].previousCounter);
-
- if( encoders_status[i].direction == FORWARD_DIRECTION)
- {
- if(difference < 0)
- {
- difference += 65535;
- }
- }
- else if(difference > 0) difference -= 65535;
- encoders_status[i].globalCounter += difference;
- encoders_status[i].previousCounter = currentCounter;
- /* Speed Calcul*/
- }
- }
- return;
-}
-
-
-
-void controlMotors(unsigned short motor) {
-
- int64_t error;
- int32_t PWM;
- int i;
-
- //refreshEncoderStatus(ENCODER1 | ENCODER2 | ENCODER3 | ENCODER4);
- for(i=0;i<NUM_MOTORS;i++)
- {
- if(motor & (1<<i)) {
- //Conversion en de desiredImpulsions en nombre d'impulsions
-
-
- error = encoders_status[i].desiredPosition - encoders_status[i].globalCounter;
- PWM = (int32_t)(error*Kp[i]);
-
- setVelocity(1<<i, PWM);
- encoders_status[i].rampePosition+=1;
- }
- }
- holonome.asserPosition+=1;
- return;
-}
-
-int64_t convertRadInImpulsions(float actualPosition,int i ) {
- int64_t actualImpulsions;
-
- actualImpulsions = (int64_t)(actualPosition*REDUCTION*GAINENCODER*4/(2*M_PI));
-
- return actualImpulsions;
-}
-
-float convertImpulsionsInRad(int64_t actualImpulsions,int i ) {
- float actualPosition;
-
- actualPosition = (float(actualImpulsions)/(REDUCTION*GAINENCODER*4/(2*M_PI)));
-
- return actualPosition;
-}
-
-void generateRampeMotor(unsigned short motor, float thetaMax, float vmax, float a) {
- int i,k;
- //Def des variables nécessaires à la générations des rampes
- float tmax=0; //Défintion du temps final de fin de rampe
- float T = control_refresh/1000000.0; //Défintion de la période d'échantillonnage
- float t; //Défini le temps discret
- int N=0; //Nombre de points dans le vecteur de rampe
- float tvmax=vmax/a; //Temps où l'on atteint la vitesse max
- float thetaVmax=0;
- float theta;
- float t1, t2; //Temps pour les trapèzes
- float thetaDesired;
- unsigned short signe;
-
- int64_t position;
- int64_t actualPosition;
-
- for(i=0;i<NUM_MOTORS;i++) {
- if(motor & 1<<i) {
- k=encoders_status[i].rampePosition;
- if(k==0) encoders_status[i].startPosition=encoders_status[i].globalCounter;
- actualPosition=encoders_status[i].startPosition;
- thetaDesired = thetaMax - convertImpulsionsInRad(actualPosition,i);
- if(thetaDesired<0) {
- signe=1; //On désire aller à une position antérieure
- thetaDesired = - thetaDesired;
- }
-
- //Choix du profil : triangulaire ou trapézoïdal
- thetaVmax = a*tvmax*tvmax;
- if(thetaVmax>thetaDesired) {
- //On génère un profil triangulaire
- tmax = 2*sqrt(thetaDesired/a);
-
- N=(int)(floorf(tmax/T))+1;
-
- if(N*T < tmax) N+=1; //On oublie forcément le point final
- //Définition des CI et CF
- t=(float)(k)*T;
-
- if(t<=tmax/2) {
- //On est dans la première partie de la courbe
- theta=a/2*t*t;
- }
- else {
- if(t<=tmax) {
- //On est dans la deuxième partie de la coube
- theta=-a/2*t*t+a*tmax*t+a*(tmax/2)*(tmax/2)-a/2*tmax*tmax;
- }
- else {
- theta = thetaDesired;
- }
- }
-
- position=convertRadInImpulsions(theta,i);
-
- }
- else {
- //On génère un profil trapézoïdal
- t1=vmax/a;
- tmax = thetaDesired/vmax+t1;
- t2 = tmax - t1;
- N =(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1;
-
- t=k*T;
-
- if(t<=t1) {
- //On est dans la première partie de la courbe
- theta=a/2*t*t;
- }
- else {
- if(t<=t2) {
- // On est dans la deuxième partie de la courbe
- theta=a/2*t1*t1+vmax*(t-t1);
- }
- else {
- if(t<=tmax) {
- //On est dans la troisième partie de la coube
- theta=-a/2*t*t+a*tmax*(t-t2)+a/2*(t1*t1+t2*t2)+vmax*(t2-t1);
- }
- else {
- theta = thetaDesired;
- }
- }
-
- }
- position=convertRadInImpulsions(theta,i);
-
- }
-
- //On doit maintenant convertir et incrémenter vis-à-vis de la position actuelle
-
- if(signe==1) position*=-1;
- position+=actualPosition;
- encoders_status[i].desiredPosition = position;
-
- }
-
- }
- return;
-}
-
-void generateRampeRobot(void) {
- int i,k;
- //Def des variables nécessaires à la générations des rampes
- float Pmax,vmax,a;
- float P,V;
- float Pvmax,tvmax,tmax;
- float T = control_refresh/1000000.0; //Défintion de la période d'échantillonnage
- 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 Pstart;
- float Ti[3]={0,0,0};
- float w[3]={0,0,0};
- unsigned short signe;
-
- int64_t position;
- int64_t actualPosition;
-
- k=holonome.asserPosition;
-
-
- if(k==0) {
- holonome.Xstart = holonome.X;
- holonome.Ystart = holonome.Y;
- holonome.Tstart = holonome.T;
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- encoders_status[i].startPosition=encoders_status[i].globalCounter;
- }
- }
-
- for(i=0;i<3;i++) {
- //Récolte des données
- switch (i) {
- case 0:
- // X
- Pmax = holonome.xmax;
- vmax = holonome.vXmax;
- a = holonome.ax;
- Pstart = holonome.Xstart;
- break;
- case 1:
- // Y
- Pmax = holonome.ymax;
- vmax = holonome.vYmax;
- a = holonome.ay;
- Pstart = holonome.Ystart;
- break;
- case 2:
- // T
- Pmax = holonome.Tmax;
- vmax = holonome.vTmax;
- a = holonome.aT;
- Pstart = holonome.Tstart;
- break;
- }
- desiredP=Pmax-Pstart;
- if(desiredP<0) {
- signe = 1;
- desiredP = -desiredP;
- }
- //Choix du profil : triangulaire ou trapézoïdal
- tvmax = vmax/a;
- Pvmax = a*tvmax*tvmax;
- if(Pvmax>desiredP) {
- //On génère un profil triangulaire
- tmax = 2*sqrt(desiredP/a);
- N=(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1; //On oublie forcément le point final
- //Définition des CI et CF
- t=(float)(k)*T;
-
- if(t<=tmax/2) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=tmax) {
- //On est dans la deuxième partie de la coube
- P=-a/2*t*t+a*tmax*t+a*(tmax/2)*(tmax/2)-a/2*tmax*tmax;
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- }
- }
- }
- else {
- //On génère un profil trapézoïdal
- t1=vmax/a;
- tmax = desiredP/vmax+t1;
- t2 = tmax - t1;
- N =(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1;
-
- t=k*T;
-
- if(t<=t1) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=t2) {
- // On est dans la deuxième partie de la courbe
- P=a/2*t1*t1+vmax*(t-t1);
- V=vmax;
- }
- else {
- if(t<=tmax) {
- //On est dans la troisième partie de la coube
- P=-a/2*t*t+a*tmax*(t-t2)+a/2*(t1*t1+t2*t2)+vmax*(t2-t1);
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- }
- }
-
- }
- }
- if(signe==1) V=-V;
- switch (i) {
- case 0:
- // X
- w[0] = 0;
- Ti[0]+=0;
- w[1]=(V*sqrt(3)/2)/R;
- Ti[1] += w[1]*T;
- w[2]=-w[1];
- Ti[2] += w[2]*T;
- break;
- case 1:
- // Y
- w[0] = -V/R; // -
- Ti[0] += w[0]*T;
- w[1]= +V/(2*R); //+
- Ti[1] += w[1]*T;
- w[2]=w[1];
- Ti[2] += w[2]*T;
- break;
- case 2:
- // T
- w[0] = -V*L1/R;
- Ti[0] += w[0]*T;
- w[1]= -V*L2/R;
- Ti[1] += w[1]*T;
- w[2]=-V*L3/R;;
- Ti[2] += w[2]*T;
- break;
- }
-
-
-
-
-
- }
- for(i=0;i<NUM_MOTORS-1;i++)
- {
- Ti[i]+= holonome.Tprevious[i];
- holonome.Tprevious[i] = Ti[i];
- }
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- //Moteur i
- actualPosition = encoders_status[i].startPosition;
- position = convertRadInImpulsions(Ti[i-1],i);
-
- position+=actualPosition;
-
- encoders_status[i].desiredPosition = position;
- }
- return;
-}
-
-void generateRampeRobotPlan() {
- int i,k;
- //Def des variables nécessaires à la générations des rampes
- float xmax, ymax,vmax,a;
- float P,V;
- float xVirtuel, yVirtuel;
- float Vx, Vy, theta;
- float Pvmax,tvmax,tmax;
- float T = control_refresh/1000000.0; //Défintion de la période d'échantillonnage
- 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 Xstart, Ystart, thetaRobot;
- float Ti[3]={0,0,0};
- float w[3]={0,0,0};
-
- int64_t position;
- int64_t actualPosition;
-
- k=holonome.asserPosition;
-
-
- if(k==0) {
- holonome.Xstart = holonome.X;
- holonome.Ystart = holonome.Y;
- holonome.Tstart = holonome.T;
- xmax = holonome.xmax;
- ymax = holonome.ymax;
-
- Xstart = holonome.Xstart;
- Ystart = holonome.Ystart;
-
- if((ymax-Ystart)==0) {
- if((xmax-Xstart)<0) holonome.theta=M_PI;
- else holonome.theta=0;
- }
- else holonome.theta = 2*atan((ymax-Ystart)/((xmax-Xstart)+sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart))));
-
- for(i=0; i<3;i++) {
- holonome.Tprevious[i]=0;
- }
-
- holonome.Pprevious = 0;
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- encoders_status[i].startPosition=encoders_status[i].globalCounter;
- }
- }
-
-
- //Récolte des données
- xmax = holonome.xmax;
- ymax = holonome.ymax;
- vmax = holonome.vitesse;
- a = holonome.acceleration;
- Xstart = holonome.Xstart;
- Ystart = holonome.Ystart;
- theta = holonome.theta;
- thetaRobot = holonome_odometry.T;
-
- desiredP=sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart));
-
- //Choix du profil : triangulaire ou trapézoïdal
- tvmax = vmax/a;
- Pvmax = a*tvmax*tvmax;
- if(Pvmax>desiredP) {
- //On génère un profil triangulaire
- tmax = 2*sqrt(desiredP/a);
- N=(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1; //On oublie forcément le point final
- //Définition des CI et CF
- t=(float)(k)*T;
-
- if(t<=tmax/2) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=tmax) {
- //On est dans la deuxième partie de la coube
- P=-a/2*t*t+a*tmax*t+a*(tmax/2)*(tmax/2)-a/2*tmax*tmax;
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- holonome.busy=0;
- }
- }
- }
- else {
- //On génère un profil trapézoïdal
- t1=vmax/a;
- tmax = desiredP/vmax+t1;
- t2 = tmax - t1;
- N =(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1;
-
- t=k*T;
-
- if(t<=t1) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=t2) {
- // On est dans la deuxième partie de la courbe
- P=a/2*t1*t1+vmax*(t-t1);
- V=vmax;
- }
- else {
- if(t<=tmax) {
- //On est dans la troisième partie de la coube
- P=-a/2*t*t+a*tmax*(t-t2)+a/2*(t1*t1+t2*t2)+vmax*(t2-t1);
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- holonome.busy=0;
- }
- }
-
- }
- }
-
- Vx = V*cos(theta-thetaRobot);
- Vy = V*sin(theta-thetaRobot);
-
- //Ajout des composantes de déviations
-
- xVirtuel = cos(theta)*holonome.Pprevious;
- yVirtuel = sin(theta)*holonome.Pprevious; // dans la base de la table
-
- //holonome.Pprevious = P;
- //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;
-
-
-
- for(i=0;i<3;i++) {
- Ti[i]=w[i]*T;
- }
-
-
- for(i=0;i<NUM_MOTORS-1;i++)
- {
- Ti[i]+= holonome.Tprevious[i];
- holonome.Tprevious[i] = Ti[i];
- }
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- //Moteur i
- actualPosition = encoders_status[i].startPosition;
- position = convertRadInImpulsions(Ti[i-1],i);
-
- position+=actualPosition;
-
- encoders_status[i].desiredPosition = position;
- }
- return;
-}
-
-/*
-void generateRampeRobotPlan() {
- int i,k;
- //Def des variables nécessaires à la générations des rampes
- float xmax, ymax,vmax,a;
- float P,V;
- float Vx, Vy, theta;
- float Pvmax,tvmax,tmax;
- float T = control_refresh/1000000.0; //Défintion de la période d'échantillonnage
- 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 Xstart, Ystart;
- float Ti[3]={0,0,0};
- float w[3]={0,0,0};
-
- int64_t position;
- int64_t actualPosition;
-
- k=holonome.asserPosition;
-
-
- if(k==0) {
- holonome.Xstart = holonome.X;
- holonome.Ystart = holonome.Y;
- holonome.Tstart = holonome.T;
- xmax = holonome.xmax;
- ymax = holonome.ymax;
-
- Xstart = holonome.Xstart;
- Ystart = holonome.Ystart;
-
- if((ymax-Ystart)==0) {
- if((xmax-Xstart)<0) holonome.theta=M_PI;
- else holonome.theta=0;
- }
- else holonome.theta = 2*atan((ymax-Ystart)/((xmax-Xstart)+sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart))));
-
- for(i=0; i<3;i++) {
- holonome.Tprevious[i]=0;
- }
-
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- encoders_status[i].startPosition=encoders_status[i].globalCounter;
- }
- }
-
-
- //Récolte des données
- xmax = holonome.xmax;
- ymax = holonome.ymax;
- vmax = holonome.vitesse;
- a = holonome.acceleration;
- Xstart = holonome.Xstart;
- Ystart = holonome.Ystart;
- theta = holonome.theta;
-
- desiredP=sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart));
-
- //Choix du profil : triangulaire ou trapézoïdal
- tvmax = vmax/a;
- Pvmax = a*tvmax*tvmax;
- if(Pvmax>desiredP) {
- //On génère un profil triangulaire
- tmax = 2*sqrt(desiredP/a);
- N=(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1; //On oublie forcément le point final
- //Définition des CI et CF
- t=(float)(k)*T;
-
- if(t<=tmax/2) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=tmax) {
- //On est dans la deuxième partie de la coube
- P=-a/2*t*t+a*tmax*t+a*(tmax/2)*(tmax/2)-a/2*tmax*tmax;
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- holonome.busy=0;
- }
- }
- }
- else {
- //On génère un profil trapézoïdal
- t1=vmax/a;
- tmax = desiredP/vmax+t1;
- t2 = tmax - t1;
- N =(int)(floorf(tmax/T))+1;
- if(N*T < tmax) N+=1;
-
- t=k*T;
-
- if(t<=t1) {
- //On est dans la première partie de la courbe
- P=a/2*t*t;
- V=a*t;
- }
- else {
- if(t<=t2) {
- // On est dans la deuxième partie de la courbe
- P=a/2*t1*t1+vmax*(t-t1);
- V=vmax;
- }
- else {
- if(t<=tmax) {
- //On est dans la troisième partie de la coube
- P=-a/2*t*t+a*tmax*(t-t2)+a/2*(t1*t1+t2*t2)+vmax*(t2-t1);
- V=-a*t+a*tmax;
- }
- else {
- P = desiredP;
- V=0;
- holonome.busy=0;
- }
- }
-
- }
- }
-
- Vx = V*cos(theta);
- Vy = V*sin(theta);
-
-
- w[0] = (-Vy)/R;
- w[1] = (Vx*sqrt(3)/2+Vy/2)/R;
- w[2] = (-Vx*sqrt(3)/2+Vy/2)/R;
-
-
-
- for(i=0;i<3;i++) {
- Ti[i]=w[i]*T;
- }
-
-
- for(i=0;i<NUM_MOTORS-1;i++)
- {
- Ti[i]+= holonome.Tprevious[i];
- holonome.Tprevious[i] = Ti[i];
- }
-
- for(i=1;i<NUM_MOTORS;i++)
- {
- //Moteur i
- actualPosition = encoders_status[i].startPosition;
- position = convertRadInImpulsions(Ti[i-1],i);
-
- position+=actualPosition;
-
- encoders_status[i].desiredPosition = position;
- }
- return;
-}*/
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.h b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.h
deleted file mode 100644
index 0c01924..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/asservissement.h
+++ /dev/null
@@ -1,112 +0,0 @@
-#ifndef HEADER__ASSERVISSEMENT
-#define HEADER__ASSERVISSEMENT
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-#include "hw/hw_led.h"
-#include <drv/timer.h>
-
-#include <stdlib.h>
-
-#define NUM_ENCODERS 4
-
-#define MOTOR_CONTROL1 MOTEUR2
-#define MOTOR_CONTROL2 MOTEUR3
-#define MOTOR_CONTROL3 MOTEUR4
-
-#define NUM_MOTORS_CONTROL 3
-
-//Paramètre de l'asservissement
-
- #define control_refresh 5000
-
-//Modèle du MOTOR1
-
- #define TAU 0
- #define KP1 0.22
- #define KP2 0.22
- #define KP3 0.22
- #define KP4 0.22
-
-//Paramètres du PID du MOTOR1
-
- #define KP 100
- #define KI 0
- #define KD 0
-
-//Paramètres des moteurs
-
- #define REDUCTION 51 //51:1
-
- #define GAINENCODER2 100
- #define GAINENCODER 360
-
-//Paramètres du robot
-
- #define R 0.052
- #define L1 0.1774
- #define L2 0.1774
- #define L3 0.1774
-
-
-
-//Def d'une structure encodeur
-
-typedef struct {
-
- int64_t globalCounter; //Somme toutes les impulsions pour connaître la position de la roue
- uint8_t direction; //Indique le sens et la direction du moteur
- float speed; //Indique la vitesse du moteur
- int32_t previousCounter; //Indique la valeur précédente du compteur issu de l'hardware
- int64_t desiredPosition; //Vector contenant la rampe suivie par la roue
- int64_t startPosition;
- int rampePosition;
- unsigned short busy;
-
-} encoder_status;
-
-typedef struct {
-
- float xmax;
- float vXmax;
- float ax;
- float ymax;
- float vYmax;
- float ay;
- float Tmax;
- float vTmax;
- float aT;
- float vitesse;
- float acceleration;
- float theta;
- int asserPosition;
- float Pprevious;
-
- //Position du robot sur la table
- float X;
- float Y;
- float T;
- float Tprevious[3];
-
- float Xstart;
- float Ystart;
- float Tstart;
-
- int busy;
-
-} robot_control;
-
-robot_control holonome;
-encoder_status encoders_status[4];
-void NORETURN control_process(void);
-
-void controlInit(void);
-void refreshEncoderStatus(unsigned short encoder);
-void controlMotors(unsigned short motors);
-int64_t convertRadInImpulsions( float desiredPosition, int i);
-float convertImpulsionsInRad(int64_t actualImpulsions,int i );
-void generateRampeMotor(unsigned short motors, float finalThetaDesired, float vmax, float a);
-void generateRampeRobot(void);
-void generateRampeRobotPlan(void);
-
-#endif
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.c
deleted file mode 100644
index 41705e7..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.c
+++ /dev/null
@@ -1,187 +0,0 @@
-#include "encoder.h"
-#include "motor.h"
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x.h"
-
-
-
-void encodersInit(void) {
-
- TIM_ICInitTypeDef TIM_ICInitStructure;
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- // Enable clocking on GPIOA, GPIOB, GPIOC and GPIOD
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC ;
-
- /* PinOut
- Encoder 1 : A1 := PA6
- B1 := PA7
-
- Encoder 2 : A2 := PC6
- B2 := PC7
-
- Encoder 3 : A3 := PA8
- B3 := PA9
-
- Encoder 4 : A4 := PB6
- B4 := PB7*/
-
- // TIM3 (A1,B1) ; TIM8 (A2,B2) ; TIM1 (A3,B3) ; TIM4 (A4,B4)
-
- // TIMx clock enable
-
- RCC->APB1ENR |= RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4;
- RCC->APB2ENR |= RCC_APB2Periph_TIM8 | RCC_APB2Periph_TIM1;
-
- // Configuring PinOut mode
-
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(6) | BV(7) | BV(8) | BV(9), GPIO_MODE_IN_FLOATING,
- GPIO_SPEED_50MHZ);
-
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOB_BASE),
- BV(6) | BV(7), GPIO_MODE_IN_FLOATING,
- GPIO_SPEED_50MHZ);
-
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOC_BASE),
- BV(6) | BV(7), GPIO_MODE_IN_FLOATING,
- GPIO_SPEED_50MHZ);
-
- // Configuring TIMx base
-
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
-
- TIM_ICStructInit(&TIM_ICInitStructure);
- TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM_ICInitStructure.TIM_ICFilter = 0x0;
-
-
-
- // Configuring chanel 1
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- // Configuring chanel 2
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- // Réglage de la configuration de l'encodeur : quadrature ou non
-
- TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM1, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
-
- // Enable encoders
-
- TIM_Cmd(TIM3, ENABLE);
- TIM_Cmd(TIM8, ENABLE);
- TIM_Cmd(TIM1, ENABLE);
- TIM_Cmd(TIM4, ENABLE);
-
- resetEncoderCount(ENCODER1 | ENCODER2 | ENCODER3 | ENCODER4 );
-
- return;
-}
-
-// Obtentions des valeurs des compteurs
-uint16_t getEncoderCount(unsigned short encoder) {
- switch (encoder) {
- case ENCODER1 :
- return TIM_GetCounter(TIM3);
- break;
- case ENCODER2 :
- return TIM_GetCounter(TIM8);
- break;
- case ENCODER3 :
- return TIM_GetCounter(TIM1);
- break;
- case ENCODER4 :
- return TIM_GetCounter(TIM4);
- break;
- }
- return 0;
-}
-
-// Reset de la valeur des compteurs à O
-void resetEncoderCount(unsigned short encoders) {
-
- if(encoders & ENCODER1) {
- TIM_SetCounter(TIM3,0);
- }
-
- if(encoders & ENCODER2) {
- TIM_SetCounter(TIM8,0);
- }
-
- if(encoders & ENCODER3) {
- TIM_SetCounter(TIM1,0);
- }
-
- if(encoders & ENCODER4) {
- TIM_SetCounter(TIM4,0);
- }
-
- return;
-}
-
-uint8_t getEncoderDirection(unsigned short encoder) {
-
- uint8_t direction;
- switch (encoder) {
- case ENCODER1 :
- if ((TIM3->CR1 & TIM_CR1_DIR)!=0) {
- direction = BACKWARD_DIRECTION;
- }
- else {
- direction = FORWARD_DIRECTION;
- }
- break;
- case ENCODER2 :
- if ((TIM8->CR1 & TIM_CR1_DIR)!=0) {
- direction = BACKWARD_DIRECTION;
- }
- else {
- direction = FORWARD_DIRECTION;
- }
- break;
- case ENCODER3 :
- if ((TIM1->CR1 & TIM_CR1_DIR)!=0) {
- direction = BACKWARD_DIRECTION;
- }
- else {
- direction = FORWARD_DIRECTION;
- }
- break;
- case ENCODER4 :
- if ((TIM4->CR1 & TIM_CR1_DIR)!=0) {
- direction = BACKWARD_DIRECTION;
- }
- else {
- direction = FORWARD_DIRECTION;
- }
- break;
- }
- return direction;
-}
-
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.h b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.h
deleted file mode 100644
index 99c7be0..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/encoder.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef HEADER__ENCODER
-#define HEADER__ENCODER
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-#include "hw/hw_led.h"
-
-#define ENCODER1 1 //encoder connected to the motor 1
-#define ENCODER2 2// ...
-#define ENCODER3 4
-#define ENCODER4 8
-
-void encodersInit(void);
-
-uint16_t getEncoderCount(unsigned short encoder);
-
-void resetEncoderCount(unsigned short encoders);
-
-uint8_t getEncoderDirection(unsigned short encoder);
-
-#endif
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.c
deleted file mode 100644
index e5a10bd..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.c
+++ /dev/null
@@ -1,168 +0,0 @@
-#include "intelligence.h"
-#include "asservissement.h"
-#include "encoder.h"
-#include "motor.h"
-#include "odometry.h"
-
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x.h"
-#include <math.h>
-
-PROC_DEFINE_STACK(stack_intelligence, KERN_MINSTACKSIZE * 4);
-
-int etape=0;
-
-void intelligenceInit(void) {
- int i;
-
- holonome.xmax=0;
- holonome.vXmax=0.25;
- holonome.ax=0.25;
- holonome.ymax=-0;
- holonome.vYmax=0.3;
- holonome.ay=0.15;
- holonome.Tmax=20*M_PI;
- holonome.vTmax=3;
- holonome.aT=0.5;
- holonome.asserPosition=0;
- holonome.X=0;
- holonome.Y=0;
- holonome.T=0;
- holonome.Xstart=0;
- holonome.Ystart=0;
- holonome.Tstart=0;
- holonome.acceleration=0;
- holonome.vitesse=0;
- holonome.busy =0;
-
- for(i=0;i<NUM_MOTORS-1;i++)
- {
- holonome.Tprevious[i]=0;
- }
-
- /* Create a new child process */
-
- proc_new(intelligence_process, NULL, sizeof(stack_intelligence), stack_intelligence);
-
-
- return;
-}
-
-void NORETURN intelligence_process(void)
-{
- Timer timer_intelligence_process;
- timer_setDelay(&timer_intelligence_process, us_to_ticks((utime_t)(intelligence_refresh)));
- timer_setEvent(&timer_intelligence_process);
-
-
-
- while(1) {
-
- timer_add(&timer_intelligence_process);// Start process timer
-
- makePath();
- timer_waitEvent(&timer_intelligence_process); // Wait until the end of counting
- }
-
-
-}
-
-void makePath(void) {
- int i;
- if(holonome.busy==0) { //On peut donc envoyer une nouvelle position
-
- holonome.X = holonome_odometry.X;
- holonome.Y = holonome_odometry.Y;
-
- //holonome.X = holonome.xmax;
- //holonome.Y = holonome.ymax;
- /*for(i=0;i<3;i++) {
- holonome.Tprevious[i]=0;
- }*/
- //holonome.asserPosition=0;
- switch(etape) {
- case 0:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = -0.5;
- holonome.ymax = 0;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 1:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = -0.5;
- holonome.ymax = 0;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 2:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = -0.5;
- holonome.ymax = -0.5;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 3:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = -0.5;
- holonome.ymax = -0.5;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 4:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = 0;
- holonome.ymax = -0.5;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 5:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = 0;
- holonome.ymax = -0.5;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 6:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = 0;
- holonome.ymax = 0;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape+=1;
- break;
- case 7:
- holonome.busy=1;
- holonome.asserPosition=0;
- holonome.xmax = 0;
- holonome.ymax = 0;
- holonome.vitesse = 0.5;
- holonome.acceleration=0.5;
- etape=0;
- break;
-
-
- }
-
-
- }
-
-
- return;
-}
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.h b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.h
deleted file mode 100644
index 42f3da2..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/intelligence.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef HEADER__INTELLIGENCE
-#define HEADER__INTELLIGENCE
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-#include "hw/hw_led.h"
-#include <drv/timer.h>
-
-#include <stdlib.h>
-
-//Paramètre de l'asservissement
-
- #define intelligence_refresh 50000
-
-void makePath(void);
-void intelligenceInit(void);
-
-void NORETURN intelligence_process(void);
-
-#endif
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.c
deleted file mode 100644
index 8cbdeb9..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.c
+++ /dev/null
@@ -1,350 +0,0 @@
-#include "motor.h"
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-
-/*Private variables */
-
- TIM_OCInitTypeDef TIM_OCInitStructure;
-
- unsigned short def_motors[4]={MOTEUR1, MOTEUR2, MOTEUR3, MOTEUR4};
- motor_status motors_status[4];
-
-void motorsInit(void) {
-
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- uint32_t SystemClock = 72000000;
- uint32_t Tim2DesiredClock = 72000000;
- uint16_t PrescalerValue = 0;
-
- int i;
-
- // Enable clocking on GPIOA, GPIOB, GPIOC and GPIOD
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC | RCC_APB2_GPIOD;
-
- // Enable clocking on TIM2
- RCC->APB1ENR |= RCC_APB1_TIM2;
- //Def I/O of motors
- //Port A
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(0) | BV(1) | BV(2) | BV(3), GPIO_MODE_AF_PP,
- GPIO_SPEED_50MHZ); //TIM2 on PWM Pins
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(5) | BV(10) , GPIO_MODE_OUT_PP,
- GPIO_SPEED_50MHZ);
-
- //Port B
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOB_BASE),
- BV(1) | BV(14) | BV(15) | BV(5) | BV(9), GPIO_MODE_OUT_PP,
- GPIO_SPEED_50MHZ);
-
- //Port C
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOC_BASE),
- BV(4) | BV(5) | BV(10) | BV(11) | BV(9), GPIO_MODE_OUT_PP,
- GPIO_SPEED_50MHZ);
-
- //Port D
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOD_BASE),
- BV(2), GPIO_MODE_OUT_PP,
- GPIO_SPEED_50MHZ);
-
- //Init of TIM2
-
-
- /*Calcul du Prescalar */
- PrescalerValue = (uint16_t) (SystemClock/Tim2DesiredClock)-1;
- //PrescalerValue = 0;
-
- /*Config du timer2 */
- TIM_TimeBaseStructure.TIM_Period = 3600; //Horloge : 20kHz
- TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-
- TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
-
- /*Initialisation des PWMs*/
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
- TIM_OCInitStructure.TIM_Pulse = 0;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
-
- /*Chanel 1*/
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OC1Init(TIM2, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
-
- /*Chanel 2*/
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OC2Init(TIM2, &TIM_OCInitStructure);
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
-
- /*Chanel 3*/
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
-
- /*Chanel 4*/
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
- TIM_OC4Init(TIM2, &TIM_OCInitStructure);
- TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
-
- TIM_ARRPreloadConfig(TIM2, ENABLE);
-
- /* TIM2 enable counter */
- TIM_Cmd(TIM2, ENABLE);
-
- /* Init motors_status*/
- for(i=0;i<NUM_MOTORS;i++)
- {
- motors_status[i].currentPWM=0;
- motors_status[i].desiredPosition=0;
- motors_status[i].currentPosition=0;
- motors_status[i].direction=0;
- motors_status[i].speed=0;
- }
-
- return;
-}
-
-void setMotors(unsigned short motors, unsigned short direction) {
- /*Def le sens de rotation des moteurs : 0 forward
- 1 backward */
- if( motors & MOTEUR1 )
- {
- switch (direction) {
- case FORWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(4), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(5), 0);
- break;
- case BACKWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(4), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(5), 1);
- break;
- }
- }
-
- if( motors & MOTEUR2 )
- {
- switch (direction) {
- case FORWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(1), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(14), 0);
- break;
- case BACKWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(1), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(14), 1);
- break;
- }
- }
-
- if( motors & MOTEUR3 )
- {
- switch (direction) {
- case FORWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(10), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(11), 0);
- break;
- case BACKWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(10), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(11), 1);
- break;
- }
- }
-
- if( motors & MOTEUR4 )
- {
- switch (direction) {
- case FORWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(5), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(9), 0);
- break;
- case BACKWARD_DIRECTION :
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(5), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(9), 1);
- break;
- }
- }
- return;
-}
-
-void enableMotors(unsigned short motors) {
- /*Active le pont H du MOTEURX */
- if( motors & MOTEUR1 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(5), 1);
- }
-
- if( motors & MOTEUR2 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(15), 1);
- }
-
- if( motors & MOTEUR3 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(10), 1);
- }
-
- if( motors & MOTEUR4 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOD_BASE), BV(2), 1);
- }
- return;
-}
-
-void disableMotors(unsigned short motors) {
- /*Active le pont H du MOTEURX */
- if( motors & MOTEUR1 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(5), 0);
- }
-
- if( motors & MOTEUR2 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(15), 0);
- }
-
- if( motors & MOTEUR3 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(10), 0);
- }
-
- if( motors & MOTEUR4 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOD_BASE), BV(2), 0);
- }
- return;
-}
-
-void setVelocity(unsigned short motors, int32_t velocity) {
-
- if(velocity > MAX_PWM) velocity=MAX_PWM;
- if(velocity < -MAX_PWM) velocity=-MAX_PWM;
-
- if( motors & MOTEUR1 )
- {
- //stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(0), 1);
- if( velocity >= 0) {
- setMotors(MOTEUR1, FORWARD_DIRECTION);
- }
- else {
- setMotors(MOTEUR1, BACKWARD_DIRECTION);
- velocity = -velocity;
- }
- if(velocity==MAX_PWM) LED1_ON();
- else LED1_OFF();
- TIM_OCInitStructure.TIM_Pulse = (uint16_t)velocity;
- TIM_OC1Init(TIM2, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
-
- }
-
- if( motors & MOTEUR2 )
- {
- //stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(1), 1);
- if( velocity >= 0) {
- setMotors(MOTEUR2, FORWARD_DIRECTION);
- }
- else {
- setMotors(MOTEUR2, BACKWARD_DIRECTION);
- velocity = -velocity;
- }
- //if(velocity==MAX_PWM) LED2_ON();
- //else LED2_OFF();
- TIM_OCInitStructure.TIM_Pulse = (uint16_t)velocity;
- TIM_OC2Init(TIM2, &TIM_OCInitStructure);
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
- }
-
- if( motors & MOTEUR3 )
- {
- //stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(2), 1);
- if( velocity >= 0) {
- setMotors(MOTEUR3, FORWARD_DIRECTION);
- }
- else {
- setMotors(MOTEUR3, BACKWARD_DIRECTION);
- velocity = -velocity;
- }
- if(velocity==MAX_PWM) LED3_ON();
- else LED3_OFF();
- TIM_OCInitStructure.TIM_Pulse = (uint16_t)velocity;
- TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
- }
-
- if( motors & MOTEUR4 )
- {
- //stm32_gpioPinWrite(((struct stm32_gpio *)GPIOA_BASE), BV(3), 1);
- if( velocity >= 0) {
- setMotors(MOTEUR4, FORWARD_DIRECTION);
- }
- else {
- setMotors(MOTEUR4, BACKWARD_DIRECTION);
- velocity = -velocity;
- }
- if(velocity==MAX_PWM) LED4_ON();
- else LED4_OFF();
- TIM_OCInitStructure.TIM_Pulse = (uint16_t)velocity;
- TIM_OC4Init(TIM2, &TIM_OCInitStructure);
- TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
- }
-
- TIM_ARRPreloadConfig(TIM2, ENABLE);
- return;
-}
-
-void breakMotors(unsigned short motors) {
- if( motors & MOTEUR1 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(4), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(5), 1);
- }
-
- if( motors & MOTEUR2 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(1), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(14), 1);
- }
-
- if( motors & MOTEUR3 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(10), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(11), 1);
- }
-
- if( motors & MOTEUR4 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(5), 1);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(9), 1);
- }
- return;
-}
-
-void freeMotors(unsigned short motors) {
- if( motors & MOTEUR1 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(4), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(5), 0);
- }
-
- if( motors & MOTEUR2 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(1), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(14), 0);
- }
-
- if( motors & MOTEUR3 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(10), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(11), 0);
- }
-
- if( motors & MOTEUR4 )
- {
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(5), 0);
- stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(9), 0);
- }
- return;
-}
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.h b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.h
deleted file mode 100644
index 8cbcf3e..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/motor.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef HEADER__MOTOR
-#define HEADER__MOTOR
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-#include "hw/hw_led.h"
-
-#include <math.h>
-
-#define NUM_MOTORS 4
-
-#define MOTEUR1 1 //Moteur à gauche de l'ascenseur vue de face
-#define MOTEUR2 2 //Moteur suivant dans le sens horaire vue de dessus
-#define MOTEUR3 4 //Moteur à droite de l'ascenseur vue de dessus
-#define MOTEUR4 8 //Moteur de l'ascenceur principal
-
-#define FORWARD_DIRECTION 0 //Incrémentation du compteur
-#define BACKWARD_DIRECTION 1 //Décrémentation du compteur
-
-#define MAX_PWM 2300
-
-//Def d'une structure moteur
-
-typedef struct {
- uint16_t currentPWM;
- float desiredPosition;
- float currentPosition;
- uint8_t direction; //Indique le sens et la direction du moteur
- float speed; //Indique la vitesse du moteur
-
-} motor_status;
-
-void motorsInit(void);
-
-//Commande des moteurs 1..4
- void setMotors(unsigned short motors, unsigned short direction);
- void enableMotors(unsigned short motors);
- void disableMotors(unsigned short motors);
- void breakMotors(unsigned short motors);
- void freeMotors(unsigned short motors);
- void setVelocity(unsigned short motors, int32_t velocity);
-
-
-#endif
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.c b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.c
deleted file mode 100644
index 0c96734..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.c
+++ /dev/null
@@ -1,132 +0,0 @@
-#include "odometry.h"
-#include "intelligence.h"
-#include "asservissement.h"
-#include "encoder.h"
-#include "motor.h"
-
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x.h"
-#include <math.h>
-
-PROC_DEFINE_STACK(stack_odometry, KERN_MINSTACKSIZE * 4);
-
-
-void odometryInit(void) {
- int i;
-
- holonome_odometry.X=0;
- holonome_odometry.Y=0;
- holonome_odometry.T=0;
-
- for(i=0;i<NUM_MOTORS-1;i++)
- {
- holonome_odometry.previousCounter[i]=0;
- }
-
- /* Create a new child process */
-
- proc_new(odometry_process, NULL, sizeof(stack_odometry), stack_odometry);
-
-
- return;
-}
-
-void NORETURN odometry_process(void)
-{
- Timer timer_odometry_process;
- timer_setDelay(&timer_odometry_process, us_to_ticks((utime_t)(odometry_refresh)));
- timer_setEvent(&timer_odometry_process);
-
-
-
- while(1) {
-
- timer_add(&timer_odometry_process);// Start process timer
- refreshOdometry();
- timer_waitEvent(&timer_odometry_process); // Wait until the end of counting
- }
-
-
-}
-/*
-void refreshOdometry() {
- int i;
- int64_t dTheta[3];
-
-
- // Rafraichissement des encodeurs
- refreshEncoderStatus(ENCODER1 | ENCODER2 | ENCODER3 | ENCODER4);
-
- // Prise en compte de la variation d'impulsions
- for(i=0; i<NUM_MOTORS;i++) {
- if(1<<i & MOTOR_CONTROL1) {
- dTheta[0] = encoders_status[i].globalCounter - holonome_odometry.previousCounter[0];
- holonome_odometry.previousCounter[0] = encoders_status[i].globalCounter;
- }
-
- if(1<<i & MOTOR_CONTROL2) {
- dTheta[1] = encoders_status[i].globalCounter - holonome_odometry.previousCounter[1];
- holonome_odometry.previousCounter[1] = encoders_status[i].globalCounter;
- }
-
- if(1<<i & MOTOR_CONTROL3) {
- dTheta[2] = encoders_status[i].globalCounter - holonome_odometry.previousCounter[2];
- holonome_odometry.previousCounter[2] = encoders_status[i].globalCounter;
- }
- }
- // T : position angulaire
- holonome_odometry.T += -R/(3*L)*(convertImpulsionsInRad( dTheta[0]+dTheta[1]+dTheta[2],0));
- if(holonome_odometry.T >= 0.01) LED2_ON();
- else LED2_OFF();
- // Y
- holonome_odometry.Y += R/3*(convertImpulsionsInRad( -2*dTheta[0]+dTheta[1]+dTheta[2],0));
- // X
- holonome_odometry.X += R/(sqrt(3))*(convertImpulsionsInRad( dTheta[1]-dTheta[2],0));
- return;
-
-}*/
-
-void refreshOdometry() {
- int i;
- float dTheta[3];
- float X1, Y1;
-
- // Rafraichissement des encodeurs
- refreshEncoderStatus(ENCODER1 | ENCODER2 | ENCODER3 | ENCODER4);
-
- // Prise en compte de la variation d'impulsions
- for(i=0; i<NUM_MOTORS;i++) {
- if(1<<i & MOTOR_CONTROL1) {
- dTheta[0] = convertImpulsionsInRad(encoders_status[i].globalCounter,0) - convertImpulsionsInRad(holonome_odometry.previousCounter[0],0);
- holonome_odometry.previousCounter[0] = encoders_status[i].globalCounter;
- }
-
- if(1<<i & MOTOR_CONTROL2) {
- dTheta[1] = convertImpulsionsInRad(encoders_status[i].globalCounter,0) - convertImpulsionsInRad(holonome_odometry.previousCounter[1],0);
- holonome_odometry.previousCounter[1] = encoders_status[i].globalCounter;
- }
-
- if(1<<i & MOTOR_CONTROL3) {
- dTheta[2] = convertImpulsionsInRad(encoders_status[i].globalCounter,0) - convertImpulsionsInRad(holonome_odometry.previousCounter[2],0);
- holonome_odometry.previousCounter[2] = encoders_status[i].globalCounter;
- }
- }
- // T : position angulaire
- holonome_odometry.T += -R/(3)*( dTheta[0]/L1+dTheta[1]/L2+dTheta[2]/L3);
- if(abs(holonome_odometry.T) >= 0.01) LED2_ON();
- else LED2_OFF();
- X1 = R/(sqrt(3))*(dTheta[1]-dTheta[2]);
- Y1 = R/3*( -2*dTheta[0]+dTheta[1]+dTheta[2]);
-
- // Y
-
- holonome_odometry.Y += sin(holonome_odometry.T)*X1+ cos(holonome_odometry.T)*Y1;
- // X
- holonome_odometry.X += cos(holonome_odometry.T)*X1- sin(holonome_odometry.T)*Y1;
- return;
-
-}
-
diff --git a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.h b/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.h
deleted file mode 100644
index be665ba..0000000
--- a/elec/boards/Balise_IR/Carte_capteurs/Firmware/controller_motor_stm32/odometry.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef HEADER__ODOMETRY
-#define HEADER__ODOMETRY
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-#include "hw/hw_led.h"
-#include <drv/timer.h>
-
-#include <stdlib.h>
-
-//Paramètre de l'asservissement
-
- #define odometry_refresh 2500
-
-typedef struct {
-
- int64_t previousCounter[3];
-
- //Position du robot sur la table
- float X;
- float Y;
- float T;
-
-
-} robot_odometry;
-
-robot_odometry holonome_odometry;
-
-
-void odometryInit(void);
-void refreshOdometry(void);
-
-void NORETURN odometry_process(void);
-
-#endif
hooks/post-receive
--
krobot
|
