Newer
Older
#include "brasLateraux.h"
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
GenericBuffer Remote::bufferRecv = GenericBuffer();
GenericBuffer Remote::bufferSend = GenericBuffer();
Arnaud Cadot
committed
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
GenericBuffer::GenericBuffer()
{
size = 0;
for(size_t i=0;i<USART_BUFFER_SIZE;++i)
buf[i] = 0;
}
bool GenericBuffer::append(uint8_t d)
{
if(size == USART_BUFFER_SIZE)
return true;
buf[size] = d;
++size;
return false;
}
uint8_t GenericBuffer::pop_front()
{
if(isEmpty())
return 0;
int d = buf[0];
--size;
memmove(buf, buf+1, size);
return d;
}
bool GenericBuffer::isEmpty() const
{
return size == 0;
}
Remote* Remote::singleton = 0;
Remote* Remote::getSingleton()
{
if (singleton==0)
singleton = new Remote();
return singleton;
}
Arnaud Cadot
committed
#ifdef ROBOTHW
initClocksAndPortsGPIO();
initUART(USART_BAUDRATE);
#endif
linSpeed = 0.;
angSpeed = 0.;
for(int i(0); i < KrabiPacket::MAX_WATCHES; i++)
mWatchesEnabled[i] = false;
mWatchesEnabled[KrabiPacket::W_POSITION] = true;
}
void Remote::initClocksAndPortsGPIO()
{
Arnaud Cadot
committed
/* Bit configuration structure for GPIOA PIN9 and PIN10 */
GPIO_InitTypeDef gpioa_init_struct;
Arnaud Cadot
committed
/* Enalbe clock for USART1, AFIO and GPIOA */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_AFIO |
RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
Arnaud Cadot
committed
/** USART remap **/
// Enable USART1 remap from PA9 and PA10 (used by USB) to PB6 and PB7
GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
Arnaud Cadot
committed
/** GPIO setup **/
GPIO_InitTypeDef GPIO_InitStructure;
Arnaud Cadot
committed
// port B pin 6 TX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
Arnaud Cadot
committed
GPIO_Init(GPIOB, &GPIO_InitStructure);
Arnaud Cadot
committed
// port B pin 7 RX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
Arnaud Cadot
committed
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
void Remote::initUART(int baudRate)
{
Arnaud Cadot
committed
/* USART configuration structure for USART1 */
USART_InitTypeDef usart1_init_struct;
/* Enable USART1 */
USART_Cmd(USART1, ENABLE);
/* Baud rate 9600, 8-bit data, One stop bit
* No parity, Do both Rx and Tx, No HW flow control
*/
usart1_init_struct.USART_BaudRate = 9600;
usart1_init_struct.USART_WordLength = USART_WordLength_8b;
usart1_init_struct.USART_StopBits = USART_StopBits_1;
usart1_init_struct.USART_Parity = USART_Parity_No ;
usart1_init_struct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
usart1_init_struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
/* Configure USART1 */
USART_Init(USART1, &usart1_init_struct);
/* Enable RXNE interrupt */
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
/* Enable USART1 global interrupt */
NVIC_EnableIRQ(USART1_IRQn);
Arnaud Cadot
committed
extern "C"
Arnaud Cadot
committed
/** Interruption handler **/
void REMOTE_USART_IRQ_HANDLER(void)
{
if(USART_GetFlagStatus(REMOTE_USART_INDEX, USART_FLAG_RXNE) != RESET) //Interuption type: 'RX register Not Empty' (i.e. data to be read)
Arnaud Cadot
committed
uint8_t d = USART_ReceiveData(REMOTE_USART_INDEX); // We can only read one byte at a time
Remote::bufferRecv.append(d);
Arnaud Cadot
committed
//Debug
Remote::getSingleton()->addData(d);
Arnaud Cadot
committed
if(USART_GetFlagStatus(REMOTE_USART_INDEX, USART_FLAG_TXE) != RESET) //Interuption type: 'TX register Empty' (i.e. ready to send)
Arnaud Cadot
committed
if(!Remote::bufferSend.isEmpty()) // There IS some data to send
USART_SendData(REMOTE_USART_INDEX, Remote::bufferSend.pop_front()); // We can only send one byte per TXE interrupt
else // We ran out of data to send
USART_ITConfig(REMOTE_USART_INDEX, USART_IT_TXE, DISABLE); // Disable the TXE interrupt
Arnaud Cadot
committed
}
Arnaud Cadot
committed
USART_ITConfig(REMOTE_USART_INDEX, USART_IT_TXE, ENABLE); // Enable the TXE interrupt
Arnaud Cadot
committed
Remote::bufferSend.append(data);
void Remote::send(KrabiPacket &packet)
{
uint8_t size = packet.length();
uint8_t* data = packet.data();
for(uint8_t i = 0; i<size; i++)
addData(0x0D);
addData(0x0A);
addData(0x0D);
addData(0x0A);
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
}
void Remote::logDirect(char* text)
{
Remote::getSingleton()->send(text);
}
void Remote::log(const char* format, ...)
{
char text[32];
va_list argptr;
va_start(argptr, format);
vsprintf(text, format, argptr);
va_end(argptr);
KrabiPacket packet(KrabiPacket::LOG_NORMAL);
packet.addString(text);
send(packet);
}
void Remote::debug(const char* format, ...)
{
char text[32];
va_list argptr;
va_start(argptr, format);
vsprintf(text, format, argptr);
va_end(argptr);
KrabiPacket packet(KrabiPacket::LOG_DEBUG);
packet.addString(text);
send(packet);
}
bool Remote::dataAvailable()
{
#ifdef ROBOTHW
return REMOTE_USART_INDEX->SR & USART_FLAG_RXNE;
#else
return false;
#endif
}
int Remote::receiveData()
{
#ifdef ROBOTHW
while (!(REMOTE_USART_INDEX->SR & USART_FLAG_RXNE));
return ((int)(REMOTE_USART_INDEX->DR & 0x1FF));
#else
return 0;
#endif
}
Arnaud Cadot
committed
while(Remote::bufferRecv.isEmpty());
update();
for(int i(0); i<20; ++i)
debug(".");
}
static long tick = 0;
tick++;
for(int i(0); i < KrabiPacket::MAX_WATCHES; i++)
if (mWatchesEnabled[i])
{
if ((KrabiPacket::W_TABLE)i == KrabiPacket::W_POSITION)
{
if (tick % 20 == i)
sendWatch((KrabiPacket::W_TABLE)i);
}
else if (tick % 10 == i)
sendWatch((KrabiPacket::W_TABLE)i);
}
/*static int test = 0;
test++;
if (test % 20 == 0)
log("test pika");*/
{
mRemoteMod = true;
static int aa = 0;
/*aa++;
if (aa % 100 == 0)*/
//log("R %d %d %d", buffer.size, buffer.buf[buffer.size - 2], buffer.buf[buffer.size - 1]);
for(int i(0); i < bufferRecv.size - 1; ++i)
if (bufferRecv.buf[i] == 0x0D && bufferRecv.buf[i + 1]== 0x0A)
//log("R %d %d %d", buffer.size, buffer.buf[buffer.size - 2], buffer.buf[buffer.size - 1]);
KrabiPacket p(bufferRecv.buf, i);
if (i < bufferRecv.size - 2)
memmove(bufferRecv.buf, bufferRecv.buf + i + 2, bufferRecv.size - (i + 2));
bufferRecv.size -= i + 2;
i = 0;
if (p.isValid())
treat(p);
}
/*static int test = 0;
test++;
if (test % 100 == 0)
{
test = 0;
Remote::getSingleton()->watch(KrabiPacket::W_POSITION, 10.f, 20.f, (float)ang);
}*/
/*if (systick_count%10 == 0)
{
PositionPlusAngle p = Odometrie::odometrie->getPos();
Remote::getSingleton()->sendWatch(KrabiPacket::W_POSITION, p.position.x, p.position.y, p.angle);
}
if (systick_count%10 == 2)
{
Remote::getSingleton()->sendWatch(KrabiPacket::W_SPEED, Odometrie::odometrie->getVitesseLineaire(), Odometrie::odometrie->getVitesseAngulaire());
}
if (systick_count%10 == 4)
{
Remote::getSingleton()->sendWatch(KrabiPacket::W_SPEED_TARGET, Asservissement::asservissement->getLinearSpeed(), Asservissement::asservissement->getAngularSpeed());
}*/
{
KrabiPacket p(KrabiPacket::TIME_SYNC);
p.add((uint16_t)StrategieV2::getTimeSpent());
Remote::getSingleton()->send(p);
}
/*if (!allowChangeMode && remoteMode && dataAvailable())
{
int order = receiveData();
Remote::log("Got:");
Remote::getSingleton()->sendData(order);
// Linear Speed
if (order>=0 and order<=50)
linSpeed = ((float)(order-25)) / 25. * LINEAR_REMOTE_SPEED_LIMIT;
// Angular Speed
if (order>=51 and order<=101)
angSpeed = -((float)(order-75)) / 25. * ANGULAR_REMOTE_SPEED_LIMIT;
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
}*/
}
void Remote::treat(KrabiPacket &packet)
{
switch(packet.id())
{
case KrabiPacket::REMOTE_MOD_SET:
mRemoteMod = true;
break;
case KrabiPacket::REMOTE_MOD_RESET:
mRemoteMod = false;
break;
case KrabiPacket::REMOTE_CONTROL_SET:
mRemoteMod = true;
mRemoteControl = true;
break;
case KrabiPacket::REMOTE_CONTROL_RESET:
mRemoteControl = false;
break;
case KrabiPacket::SET_ODOMETRIE:
{
float wheelsize = packet.get<float>();
float interaxis = packet.get<float>();
Odometrie::odometrie->setSettings(interaxis, wheelsize);
break;
}
case KrabiPacket::SET_PID_LIN:
{
float p = packet.get<float>();
float i = packet.get<float>();
float d = packet.get<float>();
bool enabled = packet.get<bool>();
Asservissement::asservissement->getPIDDistance().setSettings(p, i, d);
Asservissement::asservissement->setEnabledPIDDistance(enabled);
break;
}
case KrabiPacket::SET_PID_ANG:
{
float p = packet.get<float>();
float i = packet.get<float>();
float d = packet.get<float>();
bool enabled = packet.get<bool>();
Asservissement::asservissement->getPIDAngle().setSettings(p, i, d);
Asservissement::asservissement->setEnabledPIDAngle(enabled);
break;
}
case KrabiPacket::RUN_PID_TEST:
{
float lin = packet.get<float>();
float ang = packet.get<float>();
float limit = packet.get<float>();
uint16_t duration = packet.get<uint16_t>();
Asservissement::asservissement->runTest(duration, lin, ang, limit);
break;
}
case KrabiPacket::RUN_GOTO:
{
float x = packet.get<float>();
float y = packet.get<float>();
float speed = packet.get<float>();
StrategieV2::setCurrentGoal(Position(x, y));
else
Asservissement::asservissement->resume();
StrategieV2::addTemporaryAction(new ActionGoTo(Position(x, y)), true);
break;
}
case KrabiPacket::STOP:
{
Asservissement::asservissement->stop();
break;
}
case KrabiPacket::WATCH_REQUIRE_ONCE:
{
uint16_t w = packet.get<uint16_t>();
sendWatch((KrabiPacket::W_TABLE)w);
}
case KrabiPacket::WATCH_SET:
{
uint16_t w = packet.get<uint16_t>();
mWatchesEnabled[w] = true;
break;
}
case KrabiPacket::WATCH_RESET:
{
uint16_t w = packet.get<uint16_t>();
mWatchesEnabled[w] = false;
break;
}
case KrabiPacket::WATCH_DESELECT_ALL:
{
for(int i(0); i < KrabiPacket::MAX_WATCHES; i++)
mWatchesEnabled[i] = false;
break;
}
case KrabiPacket::TIME_RESET:
StrategieV2::resetTime();
break;
void Remote::sendWatch(KrabiPacket::W_TABLE w, int time)
KrabiPacket p(time == -1 ? KrabiPacket::WATCH_VARIABLE : KrabiPacket::WATCH_VARIABLE_TIMED, w);
if (time >= 0)
p.add((uint32_t) time);
case KrabiPacket::W_WATCHES:
{
for(int i(1); i < KrabiPacket::MAX_WATCHES; i++)
{
p.add((uint8_t) i);
p.add(mWatchesEnabled[i]);
}
break;
}
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
case KrabiPacket::W_POSITION:
{
PositionPlusAngle pos = Odometrie::odometrie->getPos();
p.add(pos.position.x);
p.add(pos.position.y);
p.add(pos.angle);
break;
}
case KrabiPacket::W_SPEED:
{
p.add(Odometrie::odometrie->getVitesseLineaire());
p.add(Odometrie::odometrie->getVitesseAngulaire());
break;
}
case KrabiPacket::W_SPEED_TARGET:
{
p.add(Asservissement::asservissement->getLinearSpeed());
p.add(Asservissement::asservissement->getAngularSpeed());
break;
}
case KrabiPacket::W_ODOMETRIE:
{
p.add(Odometrie::odometrie->getWheelSize());
p.add(Odometrie::odometrie->getInterAxisDistance());
break;
}
case KrabiPacket::W_PID_LIN:
{
p.add(Asservissement::asservissement->getPIDDistance().getKp());
p.add(Asservissement::asservissement->getPIDDistance().getKi());
p.add(Asservissement::asservissement->getPIDDistance().getKd());
break;
}
case KrabiPacket::W_PID_ANG:
{
p.add(Asservissement::asservissement->getPIDAngle().getKp());
p.add(Asservissement::asservissement->getPIDAngle().getKi());
p.add(Asservissement::asservissement->getPIDAngle().getKd());
break;
}
case KrabiPacket::W_SHARPS:
{
{
Sensor::OutputSensor out = StrategieV2::getSensors()[i]->getValue();
p.add(out.b);
}
break;
}
default:
return;
}
bool Remote::isInRemoteMod()
{
return mRemoteMod;
}
bool Remote::isInRemoteControl()
}
float Remote::getLeftPWM()
{
return linSpeed;
#if defined(STM32F40_41xxx) || defined(STM32F10X_MD)
return linSpeed + angSpeed;
#else
return linSpeed - angSpeed;
#endif
}
float Remote::getRightPWM()
{
return angSpeed;
#if defined(STM32F40_41xxx) || defined(STM32F10X_MD)
return linSpeed - angSpeed;
#else
return linSpeed + angSpeed;
#endif
}