/*
Accelerometer Thread.
Polls the acclerometer for axis data every 10ms.
Updates the global pitch and roll variables in the measurements struct
*/
void accelerometer_thread(void const *argument) {
float raw_pitch, raw_roll;
float a_x, a_y, a_z;
int32_t aggregateResult;
uint8_t buffer[6];
uint8_t x_flag, y_flag;
ring_buffer_t pitch_filter;
int size = PITCH_FILTER_SIZE;
int pitch_buffer[size];
init_buffer(&pitch_filter,pitch_buffer,size);
ring_buffer_t roll_filter;
size = ROLL_FILTER_SIZE;
int roll_buffer[size];
init_buffer(&roll_filter,roll_buffer,size);
/*
Poll the accelerometer and filter the data.
*/
while(1) {
get_pitch_roll(&raw_pitch, &raw_roll);
global_pitch = -filter_point((int) raw_pitch, &pitch_filter); //FIX
global_roll = filter_point((int) raw_roll, &roll_filter);
if (IS_TRANSMITTER) {
measurements.pitch = global_pitch;
measurements.roll = global_roll;
}
//printf("ACCR: Pitch = %f Roll = %f \n",pitch,roll);
osDelay(ACCR_DELAY);
}
}
int main( int argc, char** argv )
{
comma::command_line_options options( argc, argv );
if( options.exists( "--help" ) || options.exists( "-h" ) || argc == 1 ) { usage(); }
offset=options.value("--error-margin",0.5);
bounds=comma::csv::ascii<bounds_t>().get(options.value("--bounds",std::string("0,0,0,0,0,0")));
bool output_all = options.exists( "--output-all");
std::vector<std::string> unnamed=options.unnamed("--output-all,--verbose,-v","-.*");
std::string operation=unnamed[0];
comma::csv::options csv(options);
csv.full_xpath=true;
bool flag_exists=false;
if( operation == "stream" )
{
std::vector<std::string> fields=comma::split(csv.fields,csv.delimiter);
if( csv.fields.empty() ) { csv.fields = "t,coordinates"; }
flag_exists = csv.has_field( "flag" );
std::string bounded_string("bounded/");
for(unsigned int i=0; i<fields.size(); i++)
{
if(fields[i].substr(0,bounded_string.size())!=bounded_string)
{
fields[i]=bounded_string+fields[i];
}
}
csv.fields=comma::join( fields, csv.delimiter );
}
else if( operation == "shape" )
{
}
else
{
std::cerr << "points-grep: expected operation, got: \"" << operation << "\"" << std::endl;
return 1;
}
flag_exists = csv.has_field( "bounded/flag" );
comma::csv::input_stream<joined_point> istream(std::cin,csv);
comma::csv::output_stream<joined_point> ostream(std::cout,csv);
comma::signal_flag is_shutdown;
joined_point pq;
if(operation=="stream")
{
if(unnamed.size()<2){ usage(); }
comma::name_value::parser parser( "filename" );
comma::csv::options bounding_csv = parser.get< comma::csv::options >( unnamed[1] ); // get stream options
comma::io::istream bounding_is( comma::split(unnamed[1],';')[0], bounding_csv.binary() ? comma::io::mode::binary : comma::io::mode::ascii ); // get stream name
//bounding stream
std::deque<bounding_point> bounding_queue;
comma::csv::input_stream<bounding_point> bounding_istream(*bounding_is, bounding_csv);
comma::io::select istream_select;
comma::io::select bounding_istream_select;
istream_select.read().add(0);
istream_select.read().add(bounding_is.fd());
bounding_istream_select.read().add(bounding_is.fd());
bool next=true;
while(!is_shutdown && ( istream.ready() || ( std::cin.good() && !std::cin.eof() ) ))
{
bool bounding_data_available = bounding_istream.ready() || ( bounding_is->good() && !bounding_is->eof());
//check so we do not block
bool bounding_istream_ready=bounding_istream.ready();
bool istream_ready=istream.ready();
if(next)
{
//only check istream if we need a new point
if(!bounding_istream_ready || !istream_ready)
{
if(!bounding_istream_ready && !istream_ready)
{
istream_select.wait(boost::posix_time::milliseconds(10));
}
else
{
istream_select.check();
}
if(istream_select.read().ready(bounding_is.fd()))
{
bounding_istream_ready=true;
}
if(istream_select.read().ready(0))
{
istream_ready=true;
}
}
}
else
{
if(!bounding_istream_ready)
{
bounding_istream_select.wait(boost::posix_time::milliseconds(10));
if(bounding_istream_select.read().ready(bounding_is.fd()))
{
bounding_istream_ready=true;
}
}
}
//keep storing available bounding data
if(bounding_istream_ready)
{
const bounding_point* q = bounding_istream.read();
if( q )
{
bounding_queue.push_back(*q);
}
else
{
bounding_data_available=false;
}
}
//if we are done with the last bounded point get next
if(next)
{
if(!istream_ready) { continue; }
const joined_point* pq_ptr = istream.read();
if( !pq_ptr ) { break; }
pq=*pq_ptr;
}
//get bound
while(bounding_queue.size()>=2)
{
if( pq.bounded.timestamp < bounding_queue[1].t ) { break; }
bounding_queue.pop_front();
}
if(bounding_queue.size()<2)
{
//bound not found
//do we have more data?
if(!bounding_data_available) { break; }
next=false;
continue;
}
//bound available
next=true; //get new point on next iteration
//discard late points
if(pq.bounded.timestamp < bounding_queue[0].t)
{
continue;
}
//match
bool is_first=( pq.bounded.timestamp - bounding_queue[0].t < bounding_queue[1].t - pq.bounded.timestamp );
pq.bounding = is_first ? bounding_queue[0] : bounding_queue[1]; // assign bounding point
//filter out object points
filter_point(pq);
if(!pq.bounded.flag && !output_all)
{
continue;
}
if(flag_exists)
{
ostream.write(pq);
ostream.flush();
continue;
}
//append flag
if(ostream.is_binary())
{
ostream.write(pq,istream.binary().last());
std::cout.write( reinterpret_cast< const char* >( &pq.bounded.flag ), sizeof( comma::uint32 ) );
}
else
{
std::string line=comma::join( istream.ascii().last(), csv.delimiter );
line+=","+boost::lexical_cast<std::string>(pq.bounded.flag);
ostream.write(pq,line);
}
ostream.flush();
}
}
else if(operation=="shape")
{
while(!is_shutdown && ( istream.ready() || ( std::cin.good() && !std::cin.eof() ) ))
{
const joined_point* pq_ptr = istream.read();
if( !pq_ptr ) { break; }
pq=*pq_ptr;
//filter out object points
filter_point(pq);
if(!pq.bounded.flag && !output_all)
{
continue;
}
if(flag_exists)
{
ostream.write(pq);
ostream.flush();
continue;
}
//append flag
if(ostream.is_binary())
{
ostream.write(pq,istream.binary().last());
std::cout.write( reinterpret_cast< const char* >( &pq.bounded.flag ), sizeof( comma::uint32 ) );
}
else
{
std::string line=comma::join( istream.ascii().last(), csv.delimiter );
line+=","+boost::lexical_cast<std::string>(pq.bounded.flag);
ostream.write(pq,line);
}
ostream.flush();
}
}
return(0);
}
/*
Main Receive Thread. It reads from the wireless and checks the control part of the packet to determine
what kind of data we are receiving. There are three types PACKET_CTRL1_BEGIN, PACKET_CTRL1_PR, PACKET_CTRL1_RECORD_BEGIN.
PACKET_CTRL1_BEGIN = Receiving a desired Roll and Pitch Angle that the board should travel to over some time
PACKET_CTRL1_PR = simply update the current pitch and roll angle for the board to follow. i.e. real time angle following
PACKET_CTRL1_RECORD_BEGIN = indicate that we are receiving a sequence of angles that the board will follow.
*/
void receive_thread(void const *argument) {
uint8_t status = CC2500_CommandProbe(CC2500_READBIT, CC2500_SRX);
printf("Moved to RX (%x) \n",status);
//osDelay(500);
ring_buffer_t dist_filter;
int size = DIST_FILTER_SIZE;
int dist_buffer[size];
init_buffer(&dist_filter,dist_buffer,size);
float f1, f2, value;
uint16_t control;
uint8_t ctrl = 0;
uint8_t ctrl2 = 0;
while (1) {
control = receive_pitchroll(&f1, &f2, &ctrl2);
//Check to see what kind of Packet we have received
if (control == PACKET_CTRL1_BEGIN) {
while (ctrl != PACKET_CTRL1_END) {
receive_keypad(&ctrl, &value);
//update our global values from the recieved data
switch(ctrl) {
case PACKET_CTRL1_PITCH:
measurements.pitchIncrement = value;
break;
case PACKET_CTRL1_ROLL:
measurements.rollIncrement = value;
break;
case PACKET_CTRL1_TIME:
measurements.time = value;
break;
}
}
//printf("EX MOVE pitchIncrement = %f, rollIncrement = %f, time = %f \n",measurements.pitchIncrement,measurements.rollIncrement,measurements.time);
//Turn off Motor Follow Mode
measurements.follow = 0;
//Start OS Timer for Motor
osTimerStart(timerid_motor,100);
//Wait till follow flag has been reset
while (!measurements.follow) {
osDelay(500);
}
ctrl = 0; // reset control
}
//Update Pitch and Roll angles for motor to follow
else if (control == PACKET_CTRL1_PR) {
measurements.pitch = f1;
measurements.roll = f2;
//Return the signal strength of the wireless receiver.
float f = filter_point(get_signal_strength(), &dist_filter);
showSignalStrength(f); //updates the LEDs
//printf("READ: Pitch = %f (%f) Roll = %f (%f) Control1 = %x Pkt_index = %i Power = %f \n",f1,global_pitch,f2,global_roll,control,ctrl2,f);
}
//Begin playing Recorded Sequence
else if (control == PACKET_CTRL1_RECORD_BEGIN) {
float pitchBuffer[256];
float rollBuffer[256];
float time_interval = f1;
//start receiving the sequence and store in the buffers
receive_record_sequence(pitchBuffer,rollBuffer,&time_interval);
//printf("RECV: Record Sequence: \n");
int i;
measurements.follow = 3; // Stop Motors Following
//printf("Motors moving sequence, timedelay = %f \n",time_interval);
i = 0;
uint8_t j;
float f1, f2;
float pitchInc, rollInc;
//Playing back the Angle Sequence
while (i < 256) {
j = 0;
pitchInc = (pitchBuffer[i+1] - pitchBuffer[i]) / 10;
rollInc = (rollBuffer[i+1] - rollBuffer[i]) / 10;
f1 = pitchBuffer[i];
f2 = rollBuffer[i];
//Linear Interpolation of the Data as we received only 1/10th the actual sequence
//so we fill in the blanks using the average of two consequetive angles in the sequence.
while (j < 10) {
f1 += pitchInc;
f2 += rollInc;
motorControl(f1, f2, global_pitch, global_roll);
printf("Motors moving to angle: index = %i Pitch = %f Roll = %f \n",i,pitchBuffer[i],rollBuffer[i]);
j++;
osDelay(10);//Delay of 100 so the Loop runs at 100Hz, the rate of which the accelerometer reads.
}
//osDelay((int)time_interval);
i++;
}
osDelay(1000);
measurements.follow = 1; // Stop following
}
}
}
