Program version 1
Dependencies: FatFileSystemSD MSCFileSystem TextLCD mbed-rtos mbed
Fork of USBFlashDiskTest by
main.cpp@2:f1f6b33cd7bd, 2012-09-16 (annotated)
- Committer:
- carlos_nascimento08
- Date:
- Sun Sep 16 15:13:32 2012 +0000
- Revision:
- 2:f1f6b33cd7bd
- Parent:
- 0:22a5db2c7926
- Child:
- 3:10da2a723362
Test with roboclaw v1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chris | 0:22a5db2c7926 | 1 | #include "mbed.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 2 | #include "RoboClaw.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 3 | #include "TextLCD.h" |
chris | 0:22a5db2c7926 | 4 | #include "MSCFileSystem.h" |
carlos_nascimento08 | 2:f1f6b33cd7bd | 5 | #include "cmsis_os.h" |
chris | 0:22a5db2c7926 | 6 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 7 | #define tempo_period 50 //Periodo do PWM |
carlos_nascimento08 | 2:f1f6b33cd7bd | 8 | #define diametro_direita 42.74 //mm |
carlos_nascimento08 | 2:f1f6b33cd7bd | 9 | #define diametro_esquerda 42.67 //mm |
carlos_nascimento08 | 2:f1f6b33cd7bd | 10 | #define pi 3.1415926535897 |
carlos_nascimento08 | 2:f1f6b33cd7bd | 11 | #define velocidade_max 220.00 //cm/seg |
carlos_nascimento08 | 2:f1f6b33cd7bd | 12 | #define tam_vetor 64 |
carlos_nascimento08 | 2:f1f6b33cd7bd | 13 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 14 | TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7 |
carlos_nascimento08 | 2:f1f6b33cd7bd | 15 | RoboClaw Placa(void); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 16 | MSCFileSystem sd("sd"); |
chris | 0:22a5db2c7926 | 17 | |
chris | 0:22a5db2c7926 | 18 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 19 | //Serial Robo(p28, p27); // tx, rx |
carlos_nascimento08 | 2:f1f6b33cd7bd | 20 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 21 | AnalogIn sensor1(p15); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 22 | AnalogIn sensor2(p16); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 23 | AnalogIn sensor3(p17); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 24 | AnalogIn sensor4(p19); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 25 | |
chris | 0:22a5db2c7926 | 26 | DigitalOut led1(LED1); |
chris | 0:22a5db2c7926 | 27 | DigitalOut led2(LED2); |
chris | 0:22a5db2c7926 | 28 | DigitalOut led3(LED3); |
chris | 0:22a5db2c7926 | 29 | DigitalOut led4(LED4); |
chris | 0:22a5db2c7926 | 30 | |
chris | 0:22a5db2c7926 | 31 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 32 | //Declaracao de variaveis universal RoboClaw: |
carlos_nascimento08 | 2:f1f6b33cd7bd | 33 | char endereco = 128; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 34 | char comando = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 35 | char valor = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 36 | char check8 = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 37 | char buffer = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 38 | unsigned int aceleracao = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 39 | int velocidade = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 40 | int velocidade_e = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 41 | int velocidade_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 42 | unsigned int cmd_distancia = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 43 | unsigned int cmd_distancia_e = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 44 | unsigned int cmd_distancia_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 45 | unsigned int distancia_e = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 46 | unsigned int distancia_d = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 47 | unsigned int Kp = 65536; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 48 | unsigned int Ki = 32768; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 49 | unsigned int Kd = 16384; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 50 | unsigned int QPPS = 800; |
chris | 0:22a5db2c7926 | 51 | |
chris | 0:22a5db2c7926 | 52 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 53 | //Funcoes Universais |
carlos_nascimento08 | 2:f1f6b33cd7bd | 54 | double distancia(int y); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 55 | |
chris | 0:22a5db2c7926 | 56 | |
chris | 0:22a5db2c7926 | 57 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 58 | /* |
carlos_nascimento08 | 2:f1f6b33cd7bd | 59 | //Rotina Interrupcao Thread -> Controle de Velocidade |
carlos_nascimento08 | 2:f1f6b33cd7bd | 60 | void thread_interrompe(void const *argument) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 61 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 62 | while (true) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 63 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 64 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 65 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 66 | osDelay(0.001); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 67 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 68 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 69 | osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 70 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 71 | //Rotina Interrupcao Thread -> Atualiza vetor de tempo |
carlos_nascimento08 | 2:f1f6b33cd7bd | 72 | void vetor_atualiza(void const *argument) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 73 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 74 | while (true) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 75 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 76 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 77 | osDelay(1); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 78 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 79 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 80 | osThreadDef(vetor_atualiza, osPriorityNormal, DEFAULT_STACK_SIZE); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 81 | */ |
carlos_nascimento08 | 2:f1f6b33cd7bd | 82 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 83 | ////////////////////////////////////////////////////////////////// |
carlos_nascimento08 | 2:f1f6b33cd7bd | 84 | ////////////////////////////////////////////////////////////////// |
carlos_nascimento08 | 2:f1f6b33cd7bd | 85 | int main() |
carlos_nascimento08 | 2:f1f6b33cd7bd | 86 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 87 | //char resp = 0, string[40], imprimir = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 88 | //int i = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 89 | //unsigned int aux = 0, dist = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 90 | //float valor1 = 0, valor2 = 0, valor3 = 0, valor4 = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 91 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 92 | //DIR *d; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 93 | //struct dirent *p; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 94 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 95 | //Robo.baud(38400); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 96 | //Robo.attach(&callback); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 97 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 98 | // Comeca Thread |
carlos_nascimento08 | 2:f1f6b33cd7bd | 99 | //osThreadCreate(osThread(thread_interrompe), NULL); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 100 | //osThreadCreate(osThread(vetor_atualiza), NULL); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 101 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 102 | |
chris | 0:22a5db2c7926 | 103 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 104 | endereco = 128; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 105 | aceleracao = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 106 | velocidade_e = 150; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 107 | velocidade_d = 150; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 108 | cmd_distancia_e = 140; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 109 | cmd_distancia_d = 140; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 110 | buffer = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 111 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 112 | Placa.comando46 (endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_d, cmd_distancia_e, buffer); |
chris | 0:22a5db2c7926 | 113 | } |
carlos_nascimento08 | 2:f1f6b33cd7bd | 114 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 115 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 116 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 117 | // Funcoes auxiliares: |
carlos_nascimento08 | 2:f1f6b33cd7bd | 118 | double distancia(int y) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 119 | { |
carlos_nascimento08 | 2:f1f6b33cd7bd | 120 | double resultado = 0, x = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 121 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 122 | if(y == 1) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 123 | x = 3.3*sensor1; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 124 | else |
carlos_nascimento08 | 2:f1f6b33cd7bd | 125 | if(y == 2) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 126 | x = 3.3*sensor2; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 127 | else |
carlos_nascimento08 | 2:f1f6b33cd7bd | 128 | if(y == 3) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 129 | x = 3.3*sensor3; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 130 | else |
carlos_nascimento08 | 2:f1f6b33cd7bd | 131 | if(y == 4) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 132 | x = 3.3*sensor4; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 133 | else |
carlos_nascimento08 | 2:f1f6b33cd7bd | 134 | x = 0; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 135 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 136 | // Grafico 2: y = 2,266x6 - 25,396x5 + 114,73x4 - 268,77x3 + 349,89x2 - 251,81x + 92,04 |
carlos_nascimento08 | 2:f1f6b33cd7bd | 137 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 138 | resultado = 2.266*x*x*x*x*x*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 139 | resultado = resultado - 25.396*x*x*x*x*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 140 | resultado = resultado + 114.73*x*x*x*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 141 | resultado = resultado - 268.77*x*x*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 142 | resultado = resultado + 349.89*x*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 143 | resultado = resultado - 251.81*x; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 144 | resultado = resultado + 92.04; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 145 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 146 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 147 | if( (resultado >= 40) || (resultado < 0)) |
carlos_nascimento08 | 2:f1f6b33cd7bd | 148 | resultado = -1; |
carlos_nascimento08 | 2:f1f6b33cd7bd | 149 | |
carlos_nascimento08 | 2:f1f6b33cd7bd | 150 | return (resultado); |
carlos_nascimento08 | 2:f1f6b33cd7bd | 151 | } |