Concours voiture autonome
/
MOTEUR_LPC_TEST_HALIMI
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 PROGRAMME_MOTEUR_V1
Revision 6:2506093a6fe2, committed 2021-12-08
- Comitter:
- aslan_halimi
- Date:
- Wed Dec 08 15:42:35 2021 +0000
- Parent:
- 5:463080a3d00f
- Commit message:
- fszzfe
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 463080a3d00f -r 2506093a6fe2 main.cpp
--- a/main.cpp Tue Dec 07 16:03:13 2021 +0000
+++ b/main.cpp Wed Dec 08 15:42:35 2021 +0000
@@ -6,17 +6,17 @@
#define TOUT_DROIT 900
#define ARRET_MOTEUR_NON_VOULU 3
#define BLINKING_RATE_MS
-#define CAPTEUR_TEMPO 50
+#define CAPTEUR_TEMPO 20
#define CAN_TEMPO 50
#define MOTEUR_TEMPO 50
#include "mbed.h"
#define POS_PER_METER 43636
#define TEMPS_ARRET_MOTEUR 5000
///
-#define TARGET_VITESSE 1000 // millimètres par seconde -_- mm/s
-
-
-
+#define TARGET_VITESSE 1000 // mm/s
+
+
+
CAN can2(p30, p29,1000000);
PwmOut Servo(p23);
@@ -26,38 +26,40 @@
DigitalOut inB(p25);
DigitalOut led1(LED1);
DigitalOut led3(LED3);
-
+
void print (void);
-
+
void serial_ISR(void);
-
+
+int Bytes2Int(char data[], int position ) ;
+
void set_position(unsigned short us) ;
-
+
void send_can (float vitesse, int nbr) ;
-
+
void send_can2(bool Moteur_stopper);
-
-//int Bytes2Int(char data[], int position ) ;
-
-//float Bytes2Float( char data[], int position) ;
-
-//float Float2Bytes( char data[], float nbr) ;
-
+
+float Bytes2Float( char data[], int position) ;
+
+float Float2Bytes( char data[], float nbr) ;
+
QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_2X, QEI_INVINX_NONE );
Timer tempo ;
Timer can_timer ;
Timer mot_timer ;
Timer capteur_timer ;
Timer Probleme_arret;
-
+Timer integral_timer;
+
unsigned short pos = 1000;
float vit = 0 ;
int spam =0;
-
+
int main() {
-
+
int32_t NewPosition, position = 0, premier_probleme = 0;
- float vitesse, direction , ValServo, ValErreur, CapteurMot, CommandeMot;
+ float vitesse = 1.05,direction,ValErreur=0,ValServo,ErreurIntegral=0,CommandeMot=0,CapteurMot=0;
+ unsigned char *chptr;
char Sens = 1, Moteur_stopper = 0;
int ajustement_direction = 0;
@@ -74,6 +76,7 @@
Probleme_arret.start();
capteur_timer.start() ;
tempo.start() ;
+ integral_timer.start();
Servo.period_ms(20);
@@ -91,17 +94,17 @@
ValServo = ValServo - ajustement_direction;
}
- ValErreur = float(msg_rx.data[1]/255.0f);
-//// CommandeMot = float(msg_rx.data[1]/255.0f);
-// CommandeMot = TARGET_VITESSE;
-// CapteurMot = vitesse*1000;
-//
-// ValErreur = ((CommandeMot - CapteurMot) + (CommandeMot - ValErreur))/10000;
-// if(ValErreur>1)
-// {
-// ValErreur = 0.99f;
-// }
-//
+// CommandeMot = float(msg_rx.data[1]/255.0f);
+// ValErreur = float(msg_rx.data[1]/255.0f);
+ CommandeMot = TARGET_VITESSE;
+ CapteurMot = vitesse*1000;
+ //) + (ErreurIntegral += ValErreur*(float(integral_timer.read_ms()*1000)))
+ ValErreur = ((CommandeMot - CapteurMot)/20000)*2;
+ if(ValErreur>1)
+ {
+ ValErreur = 0.99f;
+ }
+
Sens = msg_rx.data[2] ;
inA = Sens;
inB = !Sens;
@@ -112,14 +115,14 @@
NewPosition = qei.GetPosition();
led1 = qei.Direction() == SET ? 1 : 0 ;
led3 = !led1;
- vitesse = float(abs(NewPosition - position))/(POS_PER_METER*0.05) ;
- if(spam>5)
+ vitesse = float(NewPosition - position)/(POS_PER_METER*0.020) ;
+ if(spam>50)
{
usb.printf("la pwm est de %f la position est de %lu la vitesse est de %f km/h\r\n",ValErreur, NewPosition,vitesse);
spam=0;
}
else spam++;
- position = NewPosition;
+ position = NewPosition ;
capteur_timer.reset() ;
}
@@ -170,9 +173,9 @@
// I_x = I_x + Ki * dt * ecart;
/******* Fin régulation PID ********/
-
-
-
+
+
+
void send_can (float vitesse, int nbr){
unsigned char *chptr ;
CANMessage message;
@@ -186,17 +189,17 @@
message.data[5] = char(nbr>>8) ;
message.data[6] = char(nbr>>16) ;
message.data[7] = char(nbr>>24) ;
-
+
message.id=0x400;
-// if(spam>48)
-// {
-// usb.printf("\rvitesse = %f, pos = %d\n\r", vitesse, nbr) ;
-// }
+ if(spam>48)
+ {
+ usb.printf("\rvitesse = %f, pos = %d\n\r", vitesse, nbr) ;
+ }
message.type=CANData;
message.format=CANStandard;
can2.write(message);
-
+
}
void send_can2(bool Moteur_stopper)
{
@@ -216,8 +219,8 @@
can2.write(message2);
}
-
-
+
+
void set_position(unsigned short us)
{
unsigned short period= us+20000;
@@ -230,7 +233,32 @@
tempo.reset();
}
}
-
+
+//int Bytes2Int(char data[], int position ){
+// int nbr = 0 ;
+// return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
+//}
+//
+//
+//float Float2Bytes( char data[], float nbr){
+// unsigned char* chptr ;
+// chptr = (unsigned char *) & nbr;
+// data[0]= (*chptr++);
+// data[1]= (*chptr++);
+// data[2]= (*chptr++);
+// data[3]= (*chptr);
+//
+//}
+//
+//float Bytes2Float( char data[], int position){
+// union {
+// float float_sent;
+// unsigned char bytes[4];
+// } u;
+//memcpy(u.bytes, data + position,4) ;
+//return u.float_sent ;
+//}
+
void serial_ISR(void){
char msg=usb.getc();
switch (msg){
@@ -267,30 +295,5 @@
}
-}
-
-//int Bytes2Int(char data[], int position ){
-// int nbr = 0 ;
-// return nbr+data[position] + (data[position+1]<<8) + (data[position+2]<<16) + (data[position+3]<<24) ;
-//}
-//
-//
-//float Float2Bytes( char data[], float nbr){
-// unsigned char* chptr ;
-// chptr = (unsigned char *) & nbr;
-// data[0]= (*chptr++);
-// data[1]= (*chptr++);
-// data[2]= (*chptr++);
-// data[3]= (*chptr);
-//
-//}
-//
-//float Bytes2Float( char data[], int position){
-// union {
-// float float_sent;
-// unsigned char bytes[4];
-// } u;
-//memcpy(u.bytes, data + position,4) ;
-//return u.float_sent ;
-//}
-
+ usb.printf("pos = %d us vit = %f \n",pos,vit);
+}
\ No newline at end of file