/**
* Gyro constructor using the Analog Input channel number with parameters for
* presetting the center and offset values. Bypasses calibration.
*
* @param channel The analog channel the gyro is connected to. Gyros
* can only be used on on-board Analog Inputs 0-1.
* @param center Preset uncalibrated value to use as the accumulator center value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) {
m_analog = std::make_shared<AnalogInput>(channel);
InitGyro();
m_center = center;
m_offset = offset;
m_analog->SetAccumulatorCenter(m_center);
m_analog->ResetAccumulator();
}
/**
* Gyro constructor with a precreated AnalogInput object.
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
: m_analog(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitGyro();
Calibrate();
}
}
void InitGPS()
{
CalculateRotateFact();
OSTimeDly(500);
InitEncoders();
InitGyro(GPS_TIMER_PPR / (double)GPS_TIMER_DIV);
g_X = START_X ;
g_Y = START_Y ;
InitTimerInt(GPS_TIMER, GPS_TIMER_PPR, GPS_TIMER_DIV, GPS_ISR, 0x10);
}
/**
* Gyro constructor with a precreated AnalogInput object and calibrated parameters.
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center, float offset) : m_analog(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitGyro();
m_center = center;
m_offset = offset;
m_analog->SetAccumulatorCenter(m_center);
m_analog->ResetAccumulator();
}
}
/**
* Gyro constructor with a precreated analog channel object.
* Use this constructor when the analog channel needs to be shared. There
* is no reference counting when an AnalogChannel is passed to the gyro.
* @param channel The AnalogChannel object that the gyro is connected to.
*/
Gyro::Gyro(AnalogChannel * channel)
{
m_analog = channel;
m_channelAllocated = false;
if (channel == NULL)
{
wpi_fatal(NullParameter);
}
else
{
InitGyro();
}
}
/**
* BSGyro constructor with a precreated analog channel object.
* Use this constructor when the analog channel needs to be shared. There
* is no reference counting when an AnalogInput is passed to the gyro.
* @param channel The AnalogInput object that the gyro is connected to.
*/
BSGyro::BSGyro(AnalogInput *channel)
{
m_analog = channel;
m_channelAllocated = false;
if (channel == NULL)
{
// wpi_setWPIError(NullParameter);
}
else
{
InitGyro();
}
}
/** Main program entry point. This routine contains the overall program flow, including initial
* setup of all components and the main program loop.
*/
int main(void)
{
SetupHardware();
uint8_t last_motor_cmd=10;
uint16_t counter=0;
seq = 0;
txBuffer = (uint8_t*)malloc(sizeof(packet_t));
badPkts = 0;
/* Create a regular character stream for the interface so that it can be used with the stdio.h functions */
CDC_Device_CreateStream(&VirtualSerial_CDC_Interface, &USBSerialStream);
// LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
sei();
ADCSRA |= 0x40; //start ADC conversion
InitGyro();
CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
// LEDs_ToggleLEDs(LEDS_LED2);
// uint8_t handOpen = 1;
for (;;)
{
// returns negative on failure, byte value on success
int16_t rxByte = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
if (!(rxByte < 0)){
// oddly ReceiveByte returns a 16-bit int
uint8_t byte = (uint8_t)rxByte;
// incrementally builds packets byte-by-byte
if (PKT_Decoded(byte, &rxPkt, &rxStat) != DECODE_STATUS_INCOMPLETE) {
togglePD4();
switch (rxStat.state)
{
case DECODE_STATUS_COMPLETE:
switch (rxPkt.type)
{
case PKTYPE_CMD_SET_ARM_POS:
// HL_SetBasePosition(rxPkt.payload[0]);
break;
case PKTYPE_CMD_SET_BASE_VEL:
togglePD4();
for(int i=0;i<4;i++)
echoback_payload[i] = rxPkt.payload[i];
// HL_setMotor(rxPkt.payload[1],rxPkt.payload[0],rxPkt.payload[3],rxPkt.payload[2]);
setSpeedRamps(rxPkt.payload[1],rxPkt.payload[0],rxPkt.payload[3],rxPkt.payload[2]);
last_motor_cmd=0;
// h_right, l_right, h_left, l_left
break;
case PKTYPE_CMD_SET_PWR_STATE:
break;
case PKTYPE_CMD_ZERO_GYRO:
break;
case PKTYPE_CMD_TOGGLE_KINECT:
Toggle_KIN_EN;
break;
// case PKTYPE_CMD_TOGGLE_CREATE:
// Toggle_CREATE_ON;
// if(READ_CREATE_ON) { // is the create powered?
// l_CREATE_CHRG_IND; // disable charging
// l_CREATE_PWR_EN;
// } else {
// h_CREATE_PWR_EN;
// h_CREATE_CHRG_IND;
// }
// break;
/* case PKTYPE_CMD_TOGGLE_HAND_STATE:
if (!handOpen) {
HL_OpenHand();
handOpen = 1;
} else {
HL_CloseHand();
handOpen = 0;
}
break;*/
}
break;
case DECODE_STATUS_INVALID:
badPkts++;
break;
}
rxStat.recvd = 0;
}
}
// FIXME: this const counter check should be replace by timer delta function
if ((counter % 100) ==0){
transmitMotorBuffer();
togglePD7();
}
if (counter > 1000){
togglePD6();
if(last_motor_cmd>10)
setRamps(0,0);
last_motor_cmd++;
// LEDs_ToggleLEDs(LEDS_LED2);
// transmitArmState();
// transmitGyroState();
// transmitEncoderState();
// transmitMotorState();
// transmitBattState();
counter = 0;
}
handleUSB();
// HL_UpdateState();
counter++;
}
}
/**
* Gyro constructor with only a channel.
*
* Use the default analog module slot.
*
* @param channel The analog channel the gyro is connected to.
*/
Gyro::Gyro(UINT32 channel)
{
m_analog = new AnalogChannel(channel);
m_channelAllocated = true;
InitGyro();
}
Gyro::Gyro(AnalogChannel & channel)
{
m_analog = &channel;
m_channelAllocated = false;
InitGyro();
}
int main(void)
{
int LogOn = 0;
int ComOn = 0;
int dT;
int16_t data[32];
uint32_t x;
UINT cnt;
InitSystemTick();
InitGPIO();
InitTimer();
STM_EVAL_LEDOn(LED4);
InitUART(115200);
InitPressureSensor();
InitFlowMeter();
InitAccAndMag();
InitGyro();
STM_EVAL_LEDOn(LED7); // zapnem LED7 - zelena
Delta_us();
while(1)
{
// sample period is time elapsed since previous sampling of sensors
sampleSensors(a,m,g);
// update timebase for next time
dT = Delta_us();
samplePeriod = 0.000001f * dT;
// convert gyro deg/s to rad/s
imuDegToRadV3(g);
// update AHRS
MadgwickFullAHRSUpdate(g, a, m, samplePeriod, quaternion);
if (LogOn) {
sprintf(text, "%5d%6d%6d%6d%7d%7d%7d%5d%5d%5d\r\n",
dT,
aRawData[0], aRawData[1], aRawData[2],
gRawData[0], gRawData[1], gRawData[2],
mRawData[0], mRawData[1], mRawData[2]);
f_write(&File, text, strlen(text), &cnt);
}
if(STM_EVAL_PBGetState(BUTTON_USER)) { // zmena rezimu vystupu
if (LogOn) {
// Stop logdata
LogOn = 0;
STM_EVAL_LEDOff(LED5); // zapnem LED7 - zelena
if (f_close(&File) == 0)
xprintf("Stop login data.\n\r");
else
xprintf("Error write datafile.\n\r");
} else {
// Start logdata
if (newFile() == 0)
xprintf("Start login data.\n\r");
else
xprintf("Error create datafile.\n\r");
STM_EVAL_LEDOn(LED5); // zapnem LED7 - zelena
LogOn = 1;
}
}
if (ComOn) {
data[0] = dT;
data[1] = aRawData[0];
data[2] = aRawData[1];
data[3] = aRawData[2];
data[4] = gRawData[0];
data[5] = gRawData[1];
data[6] = gRawData[2];
data[7] = mRawData[0];
data[8] = mRawData[1];
data[9] = mRawData[2];
data[10] = 0x8080;
SendDataUART((uint8_t*) data, 22);
}
if (IsReceiveUART()) {
if (ReadByteUART() == 's') {
STM_EVAL_LEDOn(LED10); // zapnem LED7 - zelena
ComOn = 1;
} else {
STM_EVAL_LEDOff(LED10); // zapnem LED7 - zelena
ComOn = 0;
}
}
/*
if (x = GetFlowMeter()) {
printf("Flow: %d\n\r", x);
}
*/
}
}
/**
* Initialize the gyro.
* Calibrate the gyro by running for a number of samples and computing the center value for this
* part. Then use the center value as the Accumulator center value for subsequent measurements.
* It's important to make sure that the robot is not moving while the centering calculations are
* in progress, this is typically done when the robot is first turned on while it's sitting at
* rest before the competition starts.
*
* @param channel The analog channel the gyro is plugged into
*/
void InitGyro(UINT32 channel)
{
InitGyro(SensorBase::GetDefaultAnalogModule(), channel);
}
/**
* Gyro constructor given a slot and a channel.
*
* @param moduleNumber The analog module the gyro is connected to (1).
* @param channel The analog channel the gyro is connected to (1 or 2).
*/
Gyro::Gyro(UINT8 moduleNumber, UINT32 channel)
{
m_analog = new AnalogChannel(moduleNumber, channel);
m_channelAllocated = true;
InitGyro();
}
/**
* BSGyro constructor with only a channel.
*
* Use the default analog module slot.
*
* @param channel The analog channel the gyro is connected to.
*/
BSGyro::BSGyro(uint32_t channel)
{
m_analog = new AnalogInput(channel);
m_channelAllocated = true;
InitGyro();
}
BSGyro::BSGyro(AnalogInput &channel)
{
m_analog = &channel;
m_channelAllocated = false;
InitGyro();
}
