main() { int fd; unsigned char* dataptr; huffmanTable stdTable = { { 0 , 1 , 1 , 1 , 1 , 1 , 1 , 0 , 3 , 0 }, { 1 , 2 , 0 , 3 , 4 , 5 , 6 , 7 , 8 }, }; int i; unsigned long long before , after; jEncoder enc = initEncoder(512,512); fd = open("prelude.pm",O_RDWR); read(fd,frame,262144); setupTables(enc,&stdTable); dmGetUST(&before); dataptr = (char*)getBitsPtr(enc); printf("%02x %02x %d\n",dataptr[55] , dataptr[56] , i); for(i = 0; i < 1000; i++) { encodeFrame(enc,(char*)frame); /* printf("%02x %02x %d\n",dataptr[55] , dataptr[56] , i);*/ } dmGetUST(&after); printf("%lld\n",(after - before)/i); printf("compressed size: %d\n",getCompressedSize(enc)); fd = open("jout",O_WRONLY|O_CREAT,0666); write(fd,getBitsPtr(enc),getCompressedSize(enc)); close(fd); }
status_t Converter::init() { status_t err = initEncoder(); if (err != OK) { releaseEncoder(); } return err; }
void VideoCapture::begin( GuiCanvas* canvas ) { // No longer waiting for a canvas mWaitingForCanvas = false; // No specified file if (mFileName.isEmpty()) { Con::errorf("VideoCapture: no file specified!"); return; } // No framegrabber, cannot capture if (mFrameGrabber == NULL) { Con::errorf("VideoCapture: cannot capture without a VideoFrameGrabber! One should be created in the GFXDevice initialization!"); return; } // Set the active encoder if (!initEncoder(mEncoderName)) return; // Store the canvas, so we know which one to capture from mCanvas = canvas; // If the resolution is zero, get the current video mode if (mResolution.isZero()) mResolution = mCanvas->getPlatformWindow()->getVideoMode().resolution; // Set the encoder file, framerate and resolution mEncoder->setFile(mFileName); mEncoder->setFramerate( &mFrameRate ); mEncoder->setResolution( &mResolution ); // The frame grabber must know about the resolution as well, since it'll do the resizing for us mFrameGrabber->setOutResolution( mResolution ); // Calculate the ms per frame mMsPerFrame = 1000.0f / mFrameRate; // Start the encoder if (!mEncoder->begin()) return; // We're now recording mIsRecording = true; mNextFramePosition = 0.0f; }
SoftAMRNBEncoder::SoftAMRNBEncoder( const char *name, const OMX_CALLBACKTYPE *callbacks, OMX_PTR appData, OMX_COMPONENTTYPE **component) : SimpleSoftOMXComponent(name, callbacks, appData, component), mEncState(NULL), mSidState(NULL), mBitRate(0), mMode(MR475), mInputSize(0), mInputTimeUs(-1ll), mSawInputEOS(false), mSignalledError(false) { initPorts(); CHECK_EQ(initEncoder(), (status_t)OK); }
void Codec_MTF::encode_MTF(DataBlock* inData) { initEncoder(inData); encodedDataSize = decodedDataSize; buffer.reserve(encodedDataSize); buffer.resize(decodedDataSize); init_mtf(256); for (unsigned int i = 0; i < decodedDataSize; ++i) buffer[i] = mtf(data[i]); inData->setData(buffer.data(), encodedDataSize); recordOutHeader(inData->getHeader(), JAA::CodecID::MTF_ID); }
void AudioConverter::feedEncoder() { int gotFrame = 0, err; uint8_t *output; int out_linesize; int avail; if (!m_encoderInitialized) initEncoder(); assert(m_avpktOutUsed == m_avpktOut.size); do { avail = avresample_available(m_resampler); av_samples_alloc(&output, &out_linesize, m_destinationFormat.mChannelsPerFrame, avail, m_encoder->sample_fmt, 0); if (avresample_read(m_resampler, &output, avail) != avail) throw std::runtime_error("avresample_read() failed"); av_init_packet(&m_avpktOut); m_avpktOut.data = 0; m_avpktOut.size = 0; m_avpktOutUsed = 0; LOG << "Got " << avail << " samples\n"; err = avcodec_fill_audio_frame(m_audioFrame, m_encoder->channels, m_encoder->sample_fmt, output, avail * m_destinationFormat.mChannelsPerFrame * (m_destinationFormat.mBitsPerChannel / 8), m_destinationFormat.mChannelsPerFrame * (m_destinationFormat.mBitsPerChannel / 8)); if (err < 0) throw std::runtime_error("avcodec_fill_audio_frame() failed"); // Encode PCM data err = avcodec_encode_audio2(m_encoder, &m_avpktOut, m_audioFrame, &gotFrame); av_freep(&output); if (err < 0) throw std::runtime_error("avcodec_encode_audio2() failed"); } while(!gotFrame); }
SoftAACEncoder2::SoftAACEncoder2( const char *name, const OMX_CALLBACKTYPE *callbacks, OMX_PTR appData, OMX_COMPONENTTYPE **component) : SimpleSoftOMXComponent(name, callbacks, appData, component), mAACEncoder(NULL), mNumChannels(1), mSampleRate(44100), mBitRate(0), mAACProfile(OMX_AUDIO_AACObjectLC), mSentCodecSpecificData(false), mInputSize(0), mInputFrame(NULL), mInputTimeUs(-1ll), mSawInputEOS(false), mSignalledError(false) { initPorts(); CHECK_EQ(initEncoder(), (status_t)OK); setAudioParams(); }
bool SMwriter_smc_old::open(FILE* file, int bits) { initVertexBuffer(1024); initEdgeBuffer(1024); vertex_hash = new my_vertex_hash; dv = new DynamicVector(); lc = new LittleCache(); initEncoder(file); initModels(1); // write precision re_conn->encode(25,bits); // store precision (is needed in write_header) precision_bits = bits; v_count = 0; f_count = 0; return true; }
Converter::Converter( const sp<AMessage> ¬ify, const sp<ALooper> &codecLooper, const sp<AMessage> &format, bool usePCMAudio) : mInitCheck(NO_INIT), mNotify(notify), mCodecLooper(codecLooper), mInputFormat(format), mIsVideo(false), mIsPCMAudio(usePCMAudio), mNeedToManuallyPrependSPSPPS(false), mDoMoreWorkPending(false) #if ENABLE_SILENCE_DETECTION ,mFirstSilentFrameUs(-1ll) ,mInSilentMode(false) #endif { AString mime; CHECK(mInputFormat->findString("mime", &mime)); if (!strncasecmp("video/", mime.c_str(), 6)) { mIsVideo = true; } CHECK(!usePCMAudio || !mIsVideo); mInitCheck = initEncoder(); if (mInitCheck != OK) { if (mEncoder != NULL) { mEncoder->release(); mEncoder.clear(); } } }
// copy mode // Basically ask a video frame and send it to writter uint8_t GenericAviSaveSmart::writeVideoChunk (uint32_t frame) { uint32_t len; uint8_t ret1, seq; //static char str[200]; if (compEngaged) // we were re-encoding { video_body->getFlags (frameStart + frame, &_videoFlag); if (_videoFlag & AVI_KEY_FRAME) { // It is a kf, go back to copy mode compEngaged = 0; stopEncoder (); // Tobias F delete _encoder; _encoder = NULL; goto cpmod; } prmod: printf ("\n %lu encoding", frame); // Else encode it .... //1-Read it ret1 = video_body->getUncompressedFrame (frameStart + frame, CPBuffer); if (!ret1) return 0; // 2-encode it if (!_encoder->encode (CPBuffer, vbuffer, &len, &_videoFlag)) return 0; // 3-write it return writter->saveVideoFrame (len, _videoFlag, vbuffer); } cpmod: // Do we have to re-encode ? ret1 = video_body->getFrameNoAlloc (frameStart + frame, vbuffer, &len, &_videoFlag, &seq); if (!ret1) return 0; // if it is neither a keyframe and there is a flow cut..... if (!(_videoFlag & AVI_KEY_FRAME) && !seq) { compEngaged = 1; // Reinit encoder initEncoder (_cqReenc); goto prmod; } // else just write it... printf ("\n %lu copying", frame); if(muxSize) { // we overshot the limit and it is a key frame // start a new chunk if(handleMuxSize() && (_videoFlag & AVI_KEY_FRAME)) { uint8_t *extraData; uint32_t extraLen; video_body->getExtraHeaderData(&extraLen,&extraData); if(!reigniteChunk(extraLen,extraData)) return 0; } } return writter->saveVideoFrame (len, _videoFlag, vbuffer); }
void main () { // Current timer state enum stateType state = COUNTING; // Default timer value to 10 minutes int timerValue = 600; // Note that count = 4x number of seconds int count = timerValue << 2; // This is used for the blinking digit during set mode char currentDigitValue = 0; lcd_init(); initTimer(); initButtons(); initEncoder(); initLedDisplay(); displayTime(timerValue); while (1) { // display the LED string displayLEDs(0, displayString[displayStringIndex]); displayLEDs(1, displayString[displayStringIndex + 1]); displayLEDs(2, displayString[displayStringIndex + 2]); displayLEDs(3, displayString[displayStringIndex + 3]); // Get user input (buttons and encoder) enum button buttonState = getButtonState(); enum encoderChange encoderState = getEncoderState(); if (state == SITTING || state == COUNTING) { // User wants to enable set mode if (buttonState == BUTTON_S2) { count = 0; timerValue = 0; currentDigitValue = 0; state = SETTING_DIGIT0; bcd.digit0 = 0xA; bcd.digit1 = 0xA; bcd.digit2 = 0xA; bcd.digit3 = 0xA; lcd_display_digits(bcd); // User wants to reset the timer } else if (buttonState == BUTTON_S3) { count = timerValue << 2; state = SITTING; displayTime(timerValue); // User wants to start the timer } else if (state == SITTING && buttonState == BUTTON_S4) { count = timerValue << 2; state = COUNTING; displayTime(count >> 2); } } // If we're already in set mode else if (state == SETTING_DIGIT0
int main() { motor_running=0; updateCtr=0; dir=1; FLASH_Unlock(); getConfig(); FLASH_Lock(); initUSART(s.usart_baud); printConfiguration(); initPWM(); initADC(); if( s.commutationMethod == commutationMethod_HALL) { initHALL(); } if(s.inputMethod == inputMethod_stepDir) { initStepDirInput(); } else if (s.inputMethod == inputMethod_pwmVelocity) { initPWMInput(); } if(s.inputMethod == inputMethod_stepDir || s.commutationMethod == commutationMethod_Encoder) { initEncoder(); } if(s.inputMethod == inputMethod_stepDir) { initPid(); } initLeds(); // errorInCommutation=1; uint8_t ena; //check if ENA is on already at start. If it is, start motor. #if ENA_POLARITY == 1 ena = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5); #else ena = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5)))&1; #endif if(ena) { pwm_motorStart(); ENABLE_LED_ON; } //two different types of main loops depending on commutation method if(s.commutationMethod == commutationMethod_Encoder) { while (1) { getEncoderCount(); if(encoder_commutation_pos != encoder_commutation_table[encoder_shaft_pos]) { //usart_sendStr("commutation to "); //usart_sendChar(encoder_commutation_table[encoder_shaft_pos]+48); encoder_commutation_pos = encoder_commutation_table[encoder_shaft_pos]; pwm_Commute(encoder_commutation_pos); // usart_sendStr("\n\r"); } } } else { while(1) { } } }
main() { char radioOutput[511]; // buffer to read the radio input from user laptop char radioInput[CINBUFSIZE]; char radioOutput2[511]; // buffer to read the radio input from instrument laptop char radioInput2[EINBUFSIZE]; // buffer to output encoder info char enc_data[70]; // buffer to relay data from the datalogger through to the radio char loggerInput[EINBUFSIZE+2]; char yetiState[10]; char axis_1[20]; char axis_2[20]; char axis_3[20]; char axis_4[20]; char temp[3]; // whether the robot has gotten a ping recently or not char ping; // the number of waypoints the robot has int numWayPoints; // the current waypoint the robot is on char curWayPoint; // interval(sec) b/w telemtry broacastings from the robot to user and to instrument char telemetryInterval; // whether gps string is valid or not int valid_gps; // whether gps navigation is engaged or not char engageGPSnav; // how many bytes in the serial buffer are used int used; int usedE; // int for indexing loops int i; // requested linear and angular velocites of robot from java int v; int w; // updated linear and angular velocites int newV; int newW; // difference between current and desired linear and angular velocites int vDiff; int wDiff; // how many characters of a gps string have been read in so far int charCounter; // character read in off the serial port (look at serFgetc() to see why it is // an int int c; // holds a gps string after reading it in char gpsString[85]; // stopping interval for data collection in seconds int resting_interval; // int to keep track of if this is the first run of the program or not int helpLast,helpLast2; //Encoder Sending code int j; int delayvar; int asb, bsb, csb; long position, asb_l, bsb_l, csb_l; float currentToGoal; //distance between current position and goal float originToCurrent; //distance between origin to current position float bearingToGoal; //bearing from current position to goal in degree float bearingToCurrent; //bearing from origin to current position in degree float INTEGRALMAX; //maximum that integral can get float deltaloop; int flag; //flag for restoring yeti to GPS navigation automatically int flag2; //flag for changing last gps coordinate // initialize hardware brdInit(); LastGPS.lat_degrees = 72; LastGPS.lat_minutes = 32.123; LastGPS.lon_degrees = 17; LastGPS.lon_minutes = 42.232; CurrentGPS.lat_degrees = 72; CurrentGPS.lat_minutes = 32.200; CurrentGPS.lon_degrees = 17; CurrentGPS.lon_minutes = 42.232; Goal.lat_degrees = 72; Goal.lat_minutes = 33.200; Goal.lon_degrees = 17; Goal.lon_minutes = 43.500; GoalPos = getPol(getCart(&CurrentGPS,&Goal)); bearingToGoal = gps_bearing2(&CurrentGPS, &Goal); CurCart = getCart(&LastGPS,&CurrentGPS); CurPol = getPol(CurCart); bearingToCurrent = gps_bearing2(&LastGPS,&CurrentGPS); error = bearRange(CurPol.t-GoalPos.t); error2 = bearRange((bearingToCurrent-bearingToGoal)*PI/180.0); printf("%f, %f\n",bearingToCurrent,bearingToGoal); printf("error:%f, error2:%f\n",error,error2); //digOutConfig(DIGOUTCONFIG); digOutConfig(0xff00); //digHoutConfig(0x07); // Set Hout0 Sinking digHTriStateConfig(0x06); // Set Hout1 & Hout2 for Tristate //initialize Encoder Board initEncoder(); //initialize state machine flags controlMode = USER_CONTROL_MODE; robotMobility = MOBILE; classificationMode = CLASSIFICATION_OFF; obstacleType = OBSTACLE_1; // open 2 radio serial ports and gps serial ports serCopen(BAUDR1); serFopen(BAUDGPS); serEopen(BAUDR2); //serEopen(BAUDLOG); //disable motor controllers //digHout(0,0); digHoutTriState(1,0); // set wheel velocities to 0 anaOutConfig(1,1); anaOutPwr(1); anaOutVolts(0,0); anaOutVolts(1,0); anaOutVolts(2,0); anaOutVolts(3,0); anaOutStrobe(); // set serial port mode serMode(0); // initialize variables ping = 1; numWayPoints = 0; coords_received = 0; //engageGPSnav = 0; //remove curWayPoint = 0; currentV = 0; currentW = 0; desiredV = 0; desiredW = 0; helpLast = 0; helpLast2 = 0; // intialize WMAX so that Vmin > Vmax / 4 (preventing robot from turning in circle) MAXW = min(2000 - AUTONOMOUS_SPEED, AUTONOMOUS_SPEED); if((AUTONOMOUS_SPEED+MAXW)/4 > AUTONOMOUS_SPEED-MAXW) MAXW = 3*AUTONOMOUS_SPEED/5; printf("MAXW : %d\n",MAXW); telemetryInterval = 1; //default telemetry broadcasting interval = 1 sec //controller coefficients P_coeff = .30; //starting point .30 I_coeff = .002; //starting point .002 loop_gain = 714; //starting point 714 INTEGRALMAX = (float)MAXW / loop_gain / I_coeff / 2.0; //integral effort max = 1/2 speed differential // clear any junk out of serial ports serFrdFlush(); serCrdFlush(); serErdFlush(); sprintf(radioOutput, "Waypoints not received, %s", gpsString); // error integration initialization last_error = 0; error_integral = 0; error = 0; error2 = 0; last_time = TICK_TIMER; deltat = 0; // stopping at each waypoint as a default setting stoppingMode = 1; resting_interval = 0; // initialize distance and bearing originToCurrent = 0; currentToGoal = 0; bearingToGoal = 0; bearingToCurrent =0; loop_time = TICK_TIMER; flag=0; flag2=0; valid_gps=-1; // main while loop while(1) { deltaloop = (float)(TICK_TIMER-loop_time)/1024; loop_time = TICK_TIMER; //Test if Serial ports have input costate { used = serCrdUsed(); //bytes being used in serial buffer for radio communication port1 usedE = serErdUsed(); //bytes being used in serial buffer for radio communication port2 or data logger } // sending data from robot to user computer #ifdef _send_telemetry_ costate sendTelemetry always_on { waitfor(DelaySec(telemetryInterval)); sprintf(radioOutput,"valid GPS: %d, ",valid_gps); serCputs(radioOutput); DelayMs(35); /*sprintf(radioOutput,"current GPS: %d,%f,%d,%f,*", CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); serCputs(radioOutput); DelayMs(35); sprintf(radioOutput,"coord GPS: %d,%f,%d,%f,*", WayPoints[0].lat_degrees, WayPoints[0].lat_minutes, WayPoints[0].lon_degrees, WayPoints[0].lon_minutes); serCputs(radioOutput); DelayMs(35);*/ sprintf(radioOutput,"errors: %f,%f, deltat: %f, ",error,error2,deltat); serCputs(radioOutput); DelayMs(35); sprintf(radioOutput,"integral term: %f, w:%d, ",I_coeff*error_integral*loop_gain,currentW); serCputs(radioOutput); DelayMs(35); sprintf(radioOutput,"dis: %f,%f,*",currentToGoal,GoalPos.r); serCputs(radioOutput); DelayMs(35); /*DelayMs(35); sprintf(radioOutput2,"loop:%f,*",deltaloop); serCputs(radioOutput2);*/ sprintf(radioOutput,"%d,%f,%d,%f,", CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); serCputs(radioOutput); DelayMs(35); sprintf(radioOutput,"%d,%d,%d,",currentV,currentW,controlMode); serCputs(radioOutput); DelayMs(35); sprintf(radioOutput,"%d,%f,%d,%f,*", Goal.lat_degrees, Goal.lat_minutes, Goal.lon_degrees, Goal.lon_minutes); serCputs(radioOutput); } costate sendTelemetry2 always_on { waitfor(DelaySec(telemetryInterval)); //send current moving status of robot to instrument package laptop sprintf(radioOutput2,"%d,%f,%d,%f,", CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); serEputs(radioOutput2); DelayMs(35); sprintf(radioOutput2,"%d,%d,%d,*",currentV,currentW,controlMode); serEputs(radioOutput2); DelayMs(35); sprintf(radioOutput2,"stopmode: %d, controlmode: %d,*",stoppingMode, controlMode); serEputs(radioOutput2); } #endif //*********************************************************************************** // serial message interpretation costate costate serialIn always_on { // wait until a message comes waitfor(used > 0); // give the message a chance to finish sending waitfor(DelayMs(100)); if(used > 100) waitfor(DelaySec(2)); //increase in case receiving long waypoint list used = serCrdUsed(); serCread(radioInput,used, 2); radioInput[used] = '\0'; //terminate string // since a message came, we know we are in radio contact ping = 1; serCrdFlush(); //remote control mode // [zeros] [v byte 1] [v byte 2] [w byte 1] [w byte 2] if(radioInput[0] == 0) { // recieve a remote control driving command //engageGPSnav = 0; //remove if (controlMode != ESCAPE_CONTROL_MODE) { controlMode = USER_CONTROL_MODE; error_integral = 0; last_time = TICK_TIMER; v = (int)radioInput[2]<<8; desiredV = v+radioInput[1]; w = (int)radioInput[4]<<8; desiredW = w+radioInput[3]; #ifdef _debug_ printf("command\n"); #endif } } else if (radioInput[0] == 1) //receiving gps waypoint list { // load in gps coordinates // convention N = +lat_deg +lat_min, W = +long_deg +long_min // S = -lat_deg -lat_min, E = -long_deg -long_min if(radioInput[1] > 180){ numWayPoints = 180; } else if(radioInput[1] >0){ numWayPoints = (int)(radioInput[1]); } else{ numWayPoints = 0; } for(i = 0;i<numWayPoints;i++) { WayPoints[i].lat_degrees = CtoI(radioInput[2+i*12],radioInput[2+i*12+1]); WayPoints[i].lat_minutes = CtoF(radioInput[2+i*12+2],radioInput[2+i*12+3],radioInput[2+i*12+4],radioInput[2+i*12+5]); WayPoints[i].lon_degrees = CtoI(radioInput[2+i*12+6],radioInput[2+i*12+7]); WayPoints[i].lon_minutes = CtoF(radioInput[2+i*12+8],radioInput[2+i*12+9],radioInput[2+i*12+10],radioInput[2+i*12+11]); DelayMs(35); //print waypoint list to logfile sprintf(radioOutput,"waypoint,%d,%d,%f,%d,%f,*",i,WayPoints[i].lat_degrees,WayPoints[i].lat_minutes,WayPoints[i].lon_degrees,WayPoints[i].lon_minutes); serCputs(radioOutput); } curWayPoint = 0; Goal = WayPoints[0]; if (numWayPoints > 0){ coords_received = 1; } DelayMs(35); sprintf(radioOutput,"coords loaded,%d,*",numWayPoints); serCputs(radioOutput); //DelayMs(35); //sprintf(radioOutput,"1st pos: %d,%f,%d,%f,*", Goal.lat_degrees, Goal.lat_minutes, Goal.lon_degrees, Goal.lon_minutes); //serCputs(radioOutput); #ifdef _debug_ printf("load coord\n"); #endif } else if (radioInput[0] == 3) { // engage GPS navigation system if (numWayPoints>0){ controlMode = GPS_CONTROL_MODE; error_integral = 0; last_time = TICK_TIMER; helpLast =0; helpLast2 = 0; originToCurrent = 0; currentToGoal = 999999; sprintf(radioOutput,"GPS mode started,*",numWayPoints); serCputs(radioOutput); } #ifdef _debug_ printf("GPS\n"); #endif } else if (radioInput[0] == 4) { //set the interval for telemetry broadcasting telemetryInterval = radioInput[1]; } // { // // set the current waypoint the robot is heading for // if ((radioInput[1] <= numWayPoints) && (radioInput[1] > 0)) // curWayPoint = radioInput[1]-1; // } // Signal to test escape sequences and re-routing // Emergency Stop the robot if Escape Mode is enabled else if (radioInput[0] == 6) { controlMode = USER_CONTROL_MODE; #ifdef _debug_ printf("E-stop\n"); #endif desiredV = 0; desiredW = 0; sprintf(radioOutput,"User Control Mode,*"); serCputs(radioOutput); } // Toggle Classification mode else if (radioInput[0] == 7) { if (classificationMode == CLASSIFICATION_OFF) { #ifdef _debug_ printf("Classification on\n"); #endif classificationMode = CLASSIFICATION_ON; } else { #ifdef _debug_ printf("Classification off\n"); #endif classificationMode = CLASSIFICATION_OFF; } } //change controller coefficients else if (radioInput[0] == 8) { P_coeff = CtoF(radioInput[1],radioInput[2],radioInput[3],radioInput[4]); I_coeff = CtoF(radioInput[5],radioInput[6],radioInput[7],radioInput[8]); loop_gain = CtoF(radioInput[9],radioInput[10],radioInput[11],radioInput[12]); INTEGRALMAX = (float)MAXW / loop_gain / I_coeff / 2.0; DelayMs(35); sprintf(radioOutput,"Coefficients loaded,*"); serCputs(radioOutput); } } //this costate controls state update for the control mode. State transitions //can also take place within functions, but where it doesn't make sense make //those transitions within a function it is taken care of here. /*costate controlModeUpdate always_on { if ((controlMode == GPS_CONTROL_MODE)&&(robotMobility == IMMOBILIZED)) { controlMode = ESCAPE_CONTROL_MODE; printf("escape control set\n"); } } */ //*********************************************************************************** // serial message interpretation costate for instrument package communication costate serial2In always_on { // wait until a message comes waitfor(usedE > 0); // give the message a chance to finish sending waitfor(DelayMs(100)); usedE = serErdUsed(); serEread(radioInput2,usedE, 2); radioInput2[usedE] = '\0'; //terminate string serErdFlush(); if(radioInput2[0] == 0) { // recieve a remote control driving command //engageGPSnav = 0; //remove if (controlMode != ESCAPE_CONTROL_MODE) { controlMode = USER_CONTROL_MODE; error_integral = 0; last_time = TICK_TIMER; v = (int)radioInput2[2]<<8; desiredV = v+radioInput2[1]; w = (int)radioInput2[4]<<8; desiredW = w+radioInput2[3]; #ifdef _debug_ printf("command\n"); #endif } } else if (radioInput2[0] == 1) //Set autonomous mode { // load in gps coordinates // [1] [lattitude degrees int byte 1] [longitutde degrees int byte 2]... if(radioInput2[1] > 180){ numWayPoints = 180; } else if(radioInput2[1] >0){ numWayPoints = (int)(radioInput2[1]); } else{ numWayPoints =0; } for(i = 0;i<numWayPoints;i++) { WayPoints[i].lat_degrees = CtoI(radioInput2[2+i*12],radioInput2[2+i*12+1]); WayPoints[i].lat_minutes = CtoF(radioInput2[2+i*12+2],radioInput2[2+i*12+3],radioInput2[2+i*12+4],radioInput2[2+i*12+5]); WayPoints[i].lon_degrees = CtoI(radioInput2[2+i*12+6],radioInput2[2+i*12+7]); WayPoints[i].lon_minutes = CtoF(radioInput2[2+i*12+8],radioInput2[2+i*12+9],radioInput2[2+i*12+10],radioInput2[2+i*12+11]); } curWayPoint = 0; Goal = WayPoints[0]; if (numWayPoints > 0) coords_received=1; sprintf(radioOutput2,"numway: %d,*",radioInput2[1]); serEputs(radioOutput2); sprintf(radioOutput2,"coords loaded,*"); serEputs(radioOutput2); #ifdef _debug_ printf("load coord\n"); #endif } else if (radioInput2[0] == 3) { // engage GPS navigation system if (numWayPoints>0){ controlMode = GPS_CONTROL_MODE; error_integral = 0; last_time = TICK_TIMER; helpLast =0; helpLast2 = 0; originToCurrent = 0; currentToGoal = 999999; sprintf(radioOutput2,"GPS Mode,*"); serEputs(radioOutput2); } //engageGPSnav = 1; //remove #ifdef _debug_ printf("GPS\n"); #endif } else if (radioInput2[0] == 4) { //set the interval for telemetry broadcasting telemetryInterval = radioInput2[1]; } // Signal to test escape sequences and re-routing // Emergency Stop the robot if Escape Mode is enabled else if (radioInput2[0] == 6) { controlMode = USER_CONTROL_MODE; #ifdef _debug_ printf("E-stop\n"); #endif desiredV = 0; desiredW = 0; sprintf(radioOutput2,"User Control Mode,*"); serEputs(radioOutput2); } else if (radioInput2[0] == 8) { P_coeff = CtoF(radioInput2[1],radioInput2[2],radioInput2[3],radioInput2[4]); I_coeff = CtoF(radioInput2[5],radioInput2[6],radioInput2[7],radioInput2[8]); loop_gain = CtoF(radioInput2[9],radioInput2[10],radioInput2[11],radioInput2[12]); INTEGRALMAX = (float)MAXW / loop_gain / I_coeff / 2.0; sprintf(radioOutput2,"coefficients loaded,*"); serEputs(radioOutput2); } // restore control mode to GPS control mode from Instrument mode else if(radioInput2[0] == 9 && controlMode == INSTRUMENT_MODE) { controlMode = GPS_CONTROL_MODE; last_time = TICK_TIMER; desiredW = 0; desiredV = AUTONOMOUS_SPEED; //DelaySec(2); } // stop for each waypoint for X seconds else if(radioInput2[0] == 10) { sprintf(radioOutput2,"command 10,*"); serEputs(radioOutput2); stoppingMode = 1; resting_interval = CtoI(radioInput2[1],radioInput2[2]); } // stop the robot for X seconds right now else if(radioInput2[0] == 11) { stoppingMode = 2; resting_interval = CtoI(radioInput2[1],radioInput2[2]); controlMode = INSTRUMENT_MODE; desiredV = 0; desiredW = 0; } // restore stopping mode to 0 (non-stoppig mode) else if(radioInput2[0] == 12) { stoppingMode = 0; } } //*********************************************************************************** //if no moving signal from instrument laptop is received for designated resting interval + 2sec //assume connection with instrument laptop is broken and go back to autonomous mode costate monitorStop always_on { if(stoppingMode != 0 && controlMode == INSTRUMENT_MODE && currentV==0 && curWayPoint < numWayPoints){ waitfor(DelaySec(resting_interval+2)); //2 sec delay in addition to resting_interval if(controlMode == INSTRUMENT_MODE){ //stoppingMode = 0; //9-13-11 retain stop at each waypoint controlMode = GPS_CONTROL_MODE; last_time = TICK_TIMER; desiredW = 0; desiredV = AUTONOMOUS_SPEED; } } } //*********************************************************************************** #ifdef _debug_ costate debugHelp always_on { waitfor(DelayMs(1000)); { switch(controlMode){ case(USER_CONTROL_MODE): printf("user control, "); break; case(GPS_CONTROL_MODE): printf("gps control, "); break; case(ESCAPE_CONTROL_MODE): printf("escape ctrl., "); break; case(ESCAPE_CONFIRM_MODE): printf("escape confirm, "); break; } switch(classificationMode){ case(CLASSIFICATION_ON): printf("class. on, "); break; case(CLASSIFICATION_OFF): printf("class. off, "); break; } switch(obstacleType){ case(OBSTACLE_0): printf("obstacle 0, "); break; case(OBSTACLE_1): printf("obstacle 1, "); break; } switch(robotMobility){ case(MOBILE): printf("mobile , "); break; case(ALMOST_IMMOBILIZED): printf("almost immob., "); break; case(IMMOBILIZED): printf("immobilized, "); break; } printf("\n"); } } #endif /* ********************************************************************************** //this costate interprets the results from the mobility and obstacle detection //classifiers //this function is disabled in this version of yeti code because we need a serial port //for a second radio for the communication with instrument package latop costate loggerInterpret always_on { // wait for a message from datalogger port waitfor(usedE > 0); // give the message a chance to finish sending waitfor(DelayMs(8)); if (classificationMode == CLASSIFICATION_ON) { usedE = serErdUsed(); serEread(loggerInput,usedE, 2); loggerInput[usedE] = '\0'; serErdFlush(); switch(loggerInput[1]) { case '0': robotMobility = MOBILE; break; case '1': robotMobility = ALMOST_IMMOBILIZED; break; case '2': robotMobility = IMMOBILIZED; break; } switch(loggerInput[2]) { case '0': obstacleType = OBSTACLE_0; break; case '1': obstacleType = OBSTACLE_1; break; default: obstacleType = OBSTACLE_1; } } else { serErdFlush(); } } */ //*********************************************************************************** // this costate updates the velocity of the wheels // acc rate is such that it takes 1 sec to go from 0 to max V // therefore can change v by at most 100 at each 50 ms time step costate { waitfor(DelayMs(50)); newV = currentV; newW = currentW; vDiff = desiredV - currentV; wDiff = desiredW - currentW; if (vDiff > 0) { newV = currentV + min(50,vDiff); //emt - changed to 50 from 100 } else if (vDiff < 0) { newV = currentV + max(-50,vDiff); } if (wDiff > 0) { newW = currentW + min(50,wDiff); } else if (wDiff < 0) { newW = currentW + max(-50,wDiff); } if ((newV != currentV) || (newW != currentW)) { //disable motor controllers if velocity should be 0 if (newV == 0 && newW == 0) digHoutTriState(1,0); else digHoutTriState(1,2); setVel(newV,newW); currentV = newV; currentW = newW; } } /* ********************************************************************************** // Send encoder data through serial to Datalogger needed for mobility detection // This function is disabled in this version of C code */ /*costate sendEncoder always_on { waitfor(DelayMs(45)); //send faster than 20Hz for (j = 0; j < 4; j++) { EncWrite(j, TRSFRCNTR_OL, CNT); // EncWrite(j,BP_RESET,CNT); EncWrite(j,BP_RESETB,CNT); asb = EncRead(j,DAT); asb_l = asb; bsb = EncRead(j,DAT); bsb_l = bsb; csb = EncRead(j,DAT); csb_l = csb; position = asb_l; // least significant byte position += (bsb_l << 8); position += (csb_l <<16); switch(j) { case 0: sprintf(axis_1,"%ld",position); break; case 1: sprintf(axis_2,"%ld",position); break; case 2: sprintf(axis_3,"%ld",position); break; case 3: sprintf(axis_4,"%ld",position); break; } } sprintf(enc_data,",%s,%s,%s,%s*",axis_1,axis_2,axis_3,axis_4); serEputs(enc_data); }*/ //*********************************************************************************** // stop robot if get no pings from java for 5 seconds DURING user control mode // continue GPS mode even if communication to user has been lost /*costate pingCheck always_on { if(controlMode == USER_CONTROL_MODE){ if (ping == 1) { waitfor(DelayMs(500)); ping = 0; abort; } else { waitfor(DelaySec(5)); if (ping == 1) abort; else { desiredV = 0; desiredW = 0; } } } }*/ //*********************************************************************************** // this costate will check if the GPS has sent some data or not and // call the appropriate functions to process the data costate GPSRead always_on { //printf("gps read started\n"); waitfor((serFrdUsed() > 0)); //always read GPS even during user control mode //printf("gps string came in\n"); charCounter = 0; // read until finding the beginning of a gps string then wait 2 seconds while(1) { c = serFgetc(); if (c == -1) { serFrdFlush(); abort; } else if (c == '$') { waitfor(DelayMs(20)); //wait for full message to send break; } } // now that 20 ms have passed, read in the gps string (it must all // be there by now, since so much time has passed getgps2(gpsString); #ifdef _debug_GPS_ //printf("gps: %u \n",strlen(test2)); printf("gps: %s ",gpsString); //puts(gpsString); //puts(": end gps \n"); #endif //=================================================================================== #ifdef _debug_GPS_fine_ printf("gps 2\n"); #endif // use Luke's library function to get gps position data from the // gps string valid_gps = gps_get_position(&CurrentGPS,gpsString); //num_sat = gps_get_satellites(&Satellite,gpsString); //<-can be used only during GPGSA mode printf("valid gps: %d, CurrentGPS: %d %f, %d %f\n", valid_gps, CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); //printf("# of sat: %d\n",num_sat); #ifdef _debug_GPS_fine_ printf("valid gps: %d, CurrentGPS: %d %f, %d %f\n", valid_gps, CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); #endif #ifdef _debug_GPS_fine_ printf("gps 3\n"); #endif #ifdef _debug_GPS_fine_ printf("gps 4\n"); #endif flag2=0; //valid gps=0 - success, -1 - parsing error, 1 - sentence marked invalid if(valid_gps==0 && controlMode == GPS_CONTROL_MODE){ // initialize last gps coordinate if program is just starting if(helpLast == 0) { LastGPS = CurrentGPS; helpLast = 1; } // find the x and y distances between the last and current GPS // positions of the robot CurCart = getCart(&LastGPS,&CurrentGPS); originToCurrent = gps_ground_distance(&LastGPS, &CurrentGPS); currentToGoal = gps_ground_distance(&CurrentGPS, &Goal); printf("CurrentGPS: %d %f, %d %f\n",CurrentGPS.lat_degrees, CurrentGPS.lat_minutes, CurrentGPS.lon_degrees, CurrentGPS.lon_minutes); printf("LastGPS: %d,%f,%d,%f\n", LastGPS.lat_degrees, LastGPS.lat_minutes, LastGPS.lon_degrees, LastGPS.lon_minutes); printf("originTocurrent: %f, currentToGoal: %f\n",originToCurrent, currentToGoal); if(currentToGoal>=WAYPOINT_RADIUS){ // compute bearing if there the robot traveled enough distance 2m if(originToCurrent >= 0.002 || helpLast2 == 0){ if(helpLast2 == 0) helpLast2 = 1; // set the flag so that v,w are updated in the second if statement flag2=1; // compute the bearing and distance from current pos to the next waypoint GoalPos = getPol(getCart(&CurrentGPS,&Goal)); bearingToGoal = gps_bearing2(&CurrentGPS, &Goal); // compute bearing from origin to current // if the robot is not stationary, calculate robot bearing // this is necessary because atan2 breaks if it is given 0/0 if(!(CurCart.x == 0 && CurCart.y == 0)) { CurPol = getPol(CurCart); } else { CurPol.t = GoalPos.t; } if(originToCurrent!=0) { bearingToCurrent = gps_bearing2(&LastGPS,&CurrentGPS); } else { bearingToCurrent = bearingToGoal; } LastGPS = CurrentGPS; /*sprintf(radioOutput,"bearing calculation: %f, %f\n", bearRange(CurPol.t-GoalPos.t),bearRange((bearingToCurrent-bearingToGoal)*PI/180.0)); serCputs(radioOutput); printf("bearing calculation: %f, %f\n", bearRange(CurPol.t-GoalPos.t),bearRange((bearingToCurrent-bearingToGoal)*PI/180.0));*/ } } } // if within 10m of the next way point, switch to the next waypoint // in the array, or stop of there are no more waypoints if (controlMode == GPS_CONTROL_MODE && currentToGoal >= WAYPOINT_RADIUS) { if (valid_gps == 0 && flag2==1) //0 = good GPS, -1 = bad GPS { // compute bearing error error = bearRange(CurPol.t-GoalPos.t); // bearing error using double precision calculation error2 = bearRange((bearingToCurrent-bearingToGoal)*PI/180.0); //only add up integral term when the robot is currently moving at full speed //& the robot has actually moved 1m from the last positoin if(currentV == AUTONOMOUS_SPEED) { deltat = (float)(TICK_TIMER-last_time) / (1024); //TICK_TIMER is shared unsigned long that counts every 1/1024 sec if(deltat<0) { deltat=(float)(TICK_TIMER-last_time+1024)/1024; } if(deltat<0) //invalid deltat -> set deltat=0 { deltat=0; } error_integral = error_integral + (error2 + last_error)/2*deltat; error_integral = bound(error_integral,INTEGRALMAX); } desiredW = (int)(bound(((-P_coeff*error2) - I_coeff*error_integral)*loop_gain, MAXW)); desiredV = AUTONOMOUS_SPEED; last_error = error2; last_time = TICK_TIMER; //(1024 times per second) } /*else if(valid_gps!=0) { // without valid GPS, go straight but slower (half of autonomous speed) // still under autonomous mode desiredW = 0; desiredV = AUTONOMOUS_SPEED_SLOW; abort; }*/ } } //*********************************************************************************** // change the current waypoint to the nextway point when the robot reaches // near the current waypoint costate WaypointCheck always_on { //default distance = .005 (5 meters) if (currentToGoal < WAYPOINT_RADIUS && controlMode == GPS_CONTROL_MODE && valid_gps==0) { // go straight for 2sec (travel about 4m straight) desiredW = 0; desiredV = AUTONOMOUS_SPEED; waitfor(DelaySec(2)); // stop at each waypoint when stoppingMode = 1 if(stoppingMode == 1){ controlMode = INSTRUMENT_MODE; desiredV = 0; desiredW = 0; } curWayPoint++; // if at the last way point, stop if(curWayPoint >= numWayPoints) { controlMode = USER_CONTROL_MODE; desiredV = 0; desiredW = 0; curWayPoint = 0; Goal = WayPoints[0]; abort; } else{ Goal = WayPoints[curWayPoint]; currentToGoal = gps_ground_distance(&CurrentGPS, &Goal); error_integral = 0; //reset error integral for PI control last_time = TICK_TIMER; //(1024 times per second) } } } //*********************************************************************************** // Change to user control mode when there is no good GPS connection /*costate GPSPingCheck always_on { waitfor(valid_gps != 0); waitfor(DelaySec(10) || (valid_gps == 0)); if (valid_gps != 0) { controlMode = USER_CONTROL_MODE; desiredV = 0; desiredW = 0; flag=1; } if(valid_gps == 0 && flag==1){ controlMode = GPS_CONTROL_MODE; flag=0; } }*/ //*********************************************************************************** //Escape mode disabled in this version because datalogger is not being used /*costate escapeSequence always_on { waitfor(controlMode == ESCAPE_CONTROL_MODE); desiredV = 0; desiredW = 0; setVel(0,0); //printf("waiting for escape confirm... \n"); //waitfor(controlMode == ESCAPE_CONFIRM_MODE); printf("escape sequence initiated \n"); switch(obstacleType){ case OBSTACLE_0: desiredV = 0; desiredW = 0; waitfor(DelayMs(500)); desiredV = -300; desiredW = 0; waitfor(DelayMs(4000)); desiredV = 0; desiredW = -600; waitfor(DelayMs(2000)); desiredV = 800; desiredW = 0; waitfor(DelayMs(4000)); break; case OBSTACLE_1: desiredV = 0; desiredW = 0; waitfor(DelayMs(500)); desiredV = -400; desiredW = 0; waitfor(DelayMs(4000)); desiredV = 1500; desiredW = 0; waitfor(DelayMs(3500)); desiredV = -400; desiredW = 0; waitfor(DelayMs(4000)); desiredV = 1500; desiredW = 0; waitfor(DelayMs(3500)); break; } //relinquish control to User desiredV = 0; desiredW = 0; controlMode = USER_CONTROL_MODE; robotMobility = MOBILE; classificationMode = CLASSIFICATION_OFF; }*/ } }
int main(int argc, char *argv[]) { int i, j; char err[256]; size_t err_len = sizeof(err); int chunkSize = 1024; int numDataUnits = 6; int numParityUnits = 3; unsigned char** dataUnits; unsigned char** parityUnits; IsalEncoder* pEncoder; int erasedIndexes[2]; unsigned char* allUnits[MMAX]; IsalDecoder* pDecoder; unsigned char* decodingOutput[2]; unsigned char** backupUnits; if (0 == build_support_erasurecode()) { printf("The native library isn't available, skipping this test\n"); return 0; // Normal, not an error } load_erasurecode_lib(err, err_len); if (strlen(err) > 0) { printf("Loading erasurecode library failed: %s\n", err); return -1; } printf("Performing erasure code test\n"); dataUnits = calloc(numDataUnits, sizeof(unsigned char*)); parityUnits = calloc(numParityUnits, sizeof(unsigned char*)); backupUnits = calloc(numParityUnits, sizeof(unsigned char*)); // Allocate and generate data units srand(135); for (i = 0; i < numDataUnits; i++) { dataUnits[i] = calloc(chunkSize, sizeof(unsigned char)); for (j = 0; j < chunkSize; j++) { dataUnits[i][j] = rand(); } } // Allocate and initialize parity units for (i = 0; i < numParityUnits; i++) { parityUnits[i] = calloc(chunkSize, sizeof(unsigned char)); for (j = 0; j < chunkSize; j++) { parityUnits[i][j] = 0; } } pEncoder = (IsalEncoder*)malloc(sizeof(IsalEncoder)); memset(pEncoder, 0, sizeof(*pEncoder)); initEncoder(pEncoder, numDataUnits, numParityUnits); encode(pEncoder, dataUnits, parityUnits, chunkSize); pDecoder = (IsalDecoder*)malloc(sizeof(IsalDecoder)); memset(pDecoder, 0, sizeof(*pDecoder)); initDecoder(pDecoder, numDataUnits, numParityUnits); memcpy(allUnits, dataUnits, numDataUnits * (sizeof (unsigned char*))); memcpy(allUnits + numDataUnits, parityUnits, numParityUnits * (sizeof (unsigned char*))); erasedIndexes[0] = 1; erasedIndexes[1] = 7; backupUnits[0] = allUnits[1]; backupUnits[1] = allUnits[7]; allUnits[0] = NULL; // Not to read allUnits[1] = NULL; allUnits[7] = NULL; decodingOutput[0] = malloc(chunkSize); decodingOutput[1] = malloc(chunkSize); decode(pDecoder, allUnits, erasedIndexes, 2, decodingOutput, chunkSize); for (i = 0; i < pDecoder->numErased; i++) { if (0 != memcmp(decodingOutput[i], backupUnits[i], chunkSize)) { fprintf(stderr, "Decoding failed\n\n"); dumpDecoder(pDecoder); return -1; } } dumpDecoder(pDecoder); fprintf(stdout, "Successfully done, passed!\n\n"); return 0; }