Carlos Alberto Nascimento Filho / Mbed 2 deprecated Andar_Linha_Reta_Robo_Claw_v1

Program version 1

Dependencies:   FatFileSystemSD MSCFileSystem TextLCD mbed-rtos mbed

Fork of USBFlashDiskTest by Chris Styles

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?

UserRevisionLine numberNew 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 }