7Robot_Freescale / Mbed 2 deprecated freescal_cup_k22f

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 freescal_cup_k22f

Dependents:   freescal_cup_k22f

source/main.cpp@25:f9d3d30cbb5d, 2015-01-21 (annotated)

Committer:
RobinN7
Date:
Wed Jan 21 10:30:39 2015 +0000
Revision:
25:f9d3d30cbb5d
Parent:
24:7b04db280873
Child:
26:a836e62e0c98
op op op

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobinN7 0:3af30bfbc3e5 1
RobinN7 0:3af30bfbc3e5 2 //Bibliothéque
RobinN7 0:3af30bfbc3e5 3 #include "mbed.h"
RobinN7 0:3af30bfbc3e5 4 #include "QEI.h"
RobinN7 0:3af30bfbc3e5 5 #include "Gestion_Moteur.h"
RobinN7 0:3af30bfbc3e5 6 #include "Camera.h"
RobinN7 0:3af30bfbc3e5 7 #include "Servo.h"
RobinN7 0:3af30bfbc3e5 8
RobinN7 0:3af30bfbc3e5 9 //Differents objet/variable global
RobinN7 0:3af30bfbc3e5 10
AlexandreN7 1:49100fa5e278 11 //test de commit
RobinN7 0:3af30bfbc3e5 12
RobinN7 14:000be67805b2 13 Serial uart(PTD3, PTD2); //xbee
RobinN7 14:000be67805b2 14 //Serial uart(USBTX, USBRX); //port série usb ACM0
RobinN7 0:3af30bfbc3e5 15 Servo servo(PTD0);
AlexandreN7 9:43c8e6d6724d 16 AnalogIn pot1(PTC1);
RobinN7 0:3af30bfbc3e5 17
RobinN7 0:3af30bfbc3e5 18 int main() {
RobinN7 0:3af30bfbc3e5 19 // Initialisation
RobinN7 0:3af30bfbc3e5 20
RobinN7 25:f9d3d30cbb5d 21 int indexMin=10;
RobinN7 25:f9d3d30cbb5d 22 int indexMax=118;
AlexandreN7 20:24ebe046ebe9 23 int compteur_uart=0;
RobinN7 21:9430357e777c 24 int max_detect1=indexMin;
RobinN7 22:b5a3688e144b 25 int min_detect2=indexMin;
RobinN7 25:f9d3d30cbb5d 26 int max_detect=(max_detect1+min_detect2)/2;
AlexandreN7 9:43c8e6d6724d 27 float Kp_servo = 0;
RobinN7 0:3af30bfbc3e5 28 uart.baud(115200);
RobinN7 0:3af30bfbc3e5 29 init_led();
RobinN7 0:3af30bfbc3e5 30
RobinN7 0:3af30bfbc3e5 31 // Init UART baudrate
RobinN7 0:3af30bfbc3e5 32
RobinN7 0:3af30bfbc3e5 33
RobinN7 0:3af30bfbc3e5 34 // Lancement boucle
RobinN7 0:3af30bfbc3e5 35 while(1){
RobinN7 25:f9d3d30cbb5d 36 double ordre;
RobinN7 0:3af30bfbc3e5 37 readline();
RobinN7 22:b5a3688e144b 38 passebas(10);
RobinN7 22:b5a3688e144b 39 derivation();
RobinN7 15:b77dc649e4f3 40 //passebas(4);
RobinN7 25:f9d3d30cbb5d 41 for (int i=0;i<15;i++)
RobinN7 25:f9d3d30cbb5d 42 {
RobinN7 25:f9d3d30cbb5d 43 pixel1[i]=0;
RobinN7 25:f9d3d30cbb5d 44 pixel2[i]=0;
RobinN7 25:f9d3d30cbb5d 45 pixel1[127-i]=0;
RobinN7 25:f9d3d30cbb5d 46 pixel2[127-i]=0;
RobinN7 25:f9d3d30cbb5d 47 }
RobinN7 25:f9d3d30cbb5d 48 if (compteur_uart ==2) // on envoit une trame toute les 50 acquisitions de cameras
AlexandreN7 20:24ebe046ebe9 49 {
AlexandreN7 20:24ebe046ebe9 50 uart.printf("S1");//debug START
AlexandreN7 20:24ebe046ebe9 51 for (int indice_pixel=0; indice_pixel<128; indice_pixel++)
AlexandreN7 20:24ebe046ebe9 52 {
AlexandreN7 20:24ebe046ebe9 53 uart.printf("%d,",pixel1[indice_pixel]);
AlexandreN7 20:24ebe046ebe9 54 }
AlexandreN7 20:24ebe046ebe9 55 uart.printf("E");//debug END
AlexandreN7 20:24ebe046ebe9 56
AlexandreN7 20:24ebe046ebe9 57 uart.printf("S2");//debug START
AlexandreN7 20:24ebe046ebe9 58 for (int indice_pixel=0; indice_pixel<128; indice_pixel++)
AlexandreN7 20:24ebe046ebe9 59 {
AlexandreN7 20:24ebe046ebe9 60 uart.printf("%d,",pixel2[indice_pixel]);
AlexandreN7 20:24ebe046ebe9 61 }
AlexandreN7 20:24ebe046ebe9 62 uart.printf("E");//debug END
AlexandreN7 20:24ebe046ebe9 63 compteur_uart =0;
RobinN7 0:3af30bfbc3e5 64 }
AlexandreN7 20:24ebe046ebe9 65 else {
AlexandreN7 20:24ebe046ebe9 66 compteur_uart =compteur_uart+1;
AlexandreN7 20:24ebe046ebe9 67 }
RobinN7 0:3af30bfbc3e5 68
RobinN7 22:b5a3688e144b 69 max_detect1=indexMin;
RobinN7 22:b5a3688e144b 70 min_detect2=indexMin;
RobinN7 22:b5a3688e144b 71
RobinN7 21:9430357e777c 72 for (int j=indexMin; j<=indexMax; j++)
RobinN7 0:3af30bfbc3e5 73 {
RobinN7 22:b5a3688e144b 74 if (pixel1[j]>pixel1[max_detect1])
RobinN7 0:3af30bfbc3e5 75 {
RobinN7 21:9430357e777c 76 max_detect1=j;
RobinN7 0:3af30bfbc3e5 77 }
RobinN7 22:b5a3688e144b 78 }
RobinN7 22:b5a3688e144b 79 for (int j=indexMin; j<=indexMax; j++)
RobinN7 22:b5a3688e144b 80 {
RobinN7 25:f9d3d30cbb5d 81 if (pixel2[j]>pixel2[min_detect2])
RobinN7 21:9430357e777c 82 {
RobinN7 22:b5a3688e144b 83 min_detect2=j;
RobinN7 21:9430357e777c 84 }
RobinN7 21:9430357e777c 85 }
RobinN7 25:f9d3d30cbb5d 86 max_detect=(max_detect1+min_detect2)/2;
RobinN7 14:000be67805b2 87 // Réduction proportionelle de la vitesse moteur si l'angle du servo augmente
RobinN7 25:f9d3d30cbb5d 88 if (max_detect>64)
RobinN7 6:a4e49784b533 89 {
RobinN7 25:f9d3d30cbb5d 90 consigne_moteur_1=6*(1-(max_detect-64)/150.);
RobinN7 25:f9d3d30cbb5d 91 consigne_moteur_2=6*(1-(max_detect-64)/100.);
RobinN7 0:3af30bfbc3e5 92 }
RobinN7 6:a4e49784b533 93 else
RobinN7 6:a4e49784b533 94 {
RobinN7 25:f9d3d30cbb5d 95 consigne_moteur_1=6*(1-(64-max_detect)/100.);
RobinN7 25:f9d3d30cbb5d 96 consigne_moteur_2=6*(1-(64-max_detect)/150.);
RobinN7 6:a4e49784b533 97 }
RobinN7 13:61c50db20069 98 // Lecture du potentiometre
RobinN7 25:f9d3d30cbb5d 99 Kp_servo=2.0*pot1.read();
RobinN7 25:f9d3d30cbb5d 100 ordre=(/*consigne*/0-((127-max_detect1)-min_detect2));//ya une merde ici
RobinN7 25:f9d3d30cbb5d 101
RobinN7 25:f9d3d30cbb5d 102
RobinN7 25:f9d3d30cbb5d 103 ordre=ordre/254+0.5;
RobinN7 13:61c50db20069 104
RobinN7 25:f9d3d30cbb5d 105 if (ordre >=0.88)
RobinN7 25:f9d3d30cbb5d 106 servo=0.88;
RobinN7 25:f9d3d30cbb5d 107 else if (ordre <=0.22)
RobinN7 25:f9d3d30cbb5d 108 servo=0.22;
RobinN7 25:f9d3d30cbb5d 109 else
RobinN7 25:f9d3d30cbb5d 110 servo= ordre;
RobinN7 25:f9d3d30cbb5d 111
RobinN7 25:f9d3d30cbb5d 112 uart.printf("a=%d, b=%d \n\r",max_detect1,min_detect2);
RobinN7 25:f9d3d30cbb5d 113
RobinN7 0:3af30bfbc3e5 114
RobinN7 25:f9d3d30cbb5d 115
RobinN7 0:3af30bfbc3e5 116 }
RobinN7 0:3af30bfbc3e5 117 }
RobinN7 0:3af30bfbc3e5 118