void joystickControl() { // Attempt to sample an event from the joystick JoystickEvent event; if (joystick->sample(&event)) { if (event.isButton()) { printf("Button %u is %s\n", event.number, event.value == 0 ? "up" : "down"); } else if (event.isAxis()) { printf("Axis %u is at position %d\n", event.number, event.value); switch(event.number) { case 0: //balanceControl.spinSpeed =(float)event.value/JOYSTICK_AXIS_MAX; smonthSpinSpeed = (float)event.value/JOYSTICK_AXIS_MAX; //balanceControl.factorL = event.value/JOYSTICK_AXIS_MAX; //balanceControl.factorR = 1-balanceControl.factorL; break; case 1: break; case 2: //右手左右 break; case 3: //右手前后 //angle = axisToAngle(event.value); //printf("angle %f\r\n", angle); //servoSetAngle(&servoH, axisToAngle(event.value)); smonthSpeed = -(float)event.value/JOYSTICK_AXIS_MAX * 10; //balanceControl.pidSpeed.desired = -(float)event.value/JOYSTICK_AXIS_MAX * 5; break; } } } balanceControl.pidSpeed.desired = LPF(balanceControl.pidSpeed.desired, smonthSpeed); balanceControl.spinSpeed = LPF(balanceControl.spinSpeed, smonthSpinSpeed); speed_need = LPF(speed_need, smonthSpeed); }
/* * change pitch effect * args: * audio_ctx - audio context * data - audio buffer to be processed * rate - window rate * * asserts: * audio_ctx is not null * * returns: none */ static void audio_fx_change_pitch (audio_context_t *audio_ctx, sample_t *data, int rate) { if(aud_fx->RT1 == NULL) { aud_fx->RT1 = calloc(1, sizeof(fx_rate_data_t)); if(aud_fx->RT1 == NULL) { fprintf(stderr,"AUDIO: FATAL memory allocation failure (audio_fx_change_pitch): %s\n", strerror(errno)); exit(-1); } aud_fx->RT1->wBuff1 = NULL; aud_fx->RT1->wBuff2 = NULL; aud_fx->RT1->rBuff1 = calloc(audio_ctx->capture_buff_size/audio_ctx->channels, sizeof(sample_t)); if(aud_fx->RT1->rBuff1 == NULL) { fprintf(stderr,"AUDIO: FATAL memory allocation failure (audio_fx_change_pitch): %s\n", strerror(errno)); exit(-1); } aud_fx->RT1->rBuff2 = NULL; if(audio_ctx->channels > 1) { aud_fx->RT1->rBuff2 = calloc(audio_ctx->capture_buff_size/audio_ctx->channels, sizeof(sample_t)); if(aud_fx->RT1->rBuff2 == NULL) { fprintf(stderr,"AUDIO: FATAL memory allocation failure (audio_fx_change_pitch): %s\n", strerror(errno)); exit(-1); } } } change_rate_less(aud_fx->RT1, data, rate, audio_ctx->capture_buff_size, audio_ctx->channels); change_tempo_more(audio_ctx, data, rate, 20); LPF(audio_ctx, data, audio_ctx->samprate * 0.25, 0.9); }
void task_application_intersection(uint16_t input) { counter++; P1OUT ^= 0x02; // toggle P1.1 for debug P4OUT ^= 0x20; // toggle P4.5 for debug P1OUT |= 0x04;P1OUT &= ~0x04; // pulse P1.2 for debug if (counter==0) { leds_circular_shift(); // circular shift LEDs for debug } //get RAM space for packet testIntersection = openqueue_getFreePacketBuffer(); //l1 testIntersection->l1_channel = DEFAULTCHANNEL; P1OUT ^= 0x02; // toggle P1.1 for debug magnetometer_get_measurement(mag_reading); mag_X = 0; mag_Y = 0; mag_Z = 0; mag_X |= mag_reading[0]<<8; mag_X |= mag_reading[1]; mag_Y |= mag_reading[2]<<8; mag_Y |= mag_reading[3]; mag_Z |= mag_reading[4]<<8; mag_Z |= mag_reading[5]; //note: in the following I use functions for simple multiplications //and divisions for easy replacements in case the number of //instruction cycles is too large to be acceptable in this application mag_X = div_int(mag_X, 970); mag_Y = div_int(mag_Y, 970); mag_Z = div_int(mag_Z, 970);//970: look in HMC5843 datasheet XX = mul_int(mag_X,mag_X); YY = mul_int(mag_Y,mag_Y); ZZ = mul_int(mag_Z,mag_Z); mag_norm = XX + YY + ZZ; //no sqrt for faster execution filt_norm = LPF(mag_norm); //here we enter the state machine switch (state) { case NOCAR: if (filt_norm>=threshold){ FSMcounter = 1; state = PERHAPS; } break; //else you break case PERHAPS: if (filt_norm >= threshold && FSMcounter < maxCount){ FSMcounter++; } else if (filt_norm < threshold && FSMcounter >minCount) FSMcounter--; else if (filt_norm < threshold && FSMcounter <=minCount){ state = NOCAR; FSMcounter=0; if(seenCar){ seenCar=0; packetfunctions_reserveHeaderSize(testIntersection,1); testIntersection->payload[0] = seenCar; packetfunctions_reserveFooterSize(testIntersection,2);//space for radio to fill in CRC //send packet(noCar) radio_send(testIntersection); } } else if (filt_norm>=threshold && FSMcounter >=maxCount){ state=CAR; if(!seenCar){ seenCar=1; packetfunctions_reserveHeaderSize(testIntersection,1); testIntersection->payload[0] = seenCar; packetfunctions_reserveFooterSize(testIntersection,2);//space for radio to fill in CRC //send packet(Car) radio_send(testIntersection); } } break; case CAR: if (filt_norm < threshold){ FSMcounter--; state = PERHAPS; } break; default: break; } }
int main(int argc, char* argv[]){ if(argc != 3){ printf("Useage: input_name output_name"); exit(0); } char file_answer; printf("Select mode:\n" "1.bin to bmp\n" "2.bmp to bmp\n" "3.bin to bin\n"); file_answer=getchar(); fflush(stdin); char *Input=argv[1], *Output=argv[2];//retrive input/output name from commandline BmpHead *pBmpHeader = new BmpHead; unsigned char **pucImageData; char *pcColorMap=NULL; if((file_answer == '1')||(file_answer == '3')){ //read bin file system("cls"); //assign header information courtersy of Lena.bin (*pBmpHeader).bfType=19778; (*pBmpHeader).bfSize=54 + 1024+ 512*512 ; // raw data size = 512*512 = 262144 bytes, modify when necessary (*pBmpHeader).bfReserved=0; (*pBmpHeader).bfOffBits=1078; (*pBmpHeader).biSize=40; (*pBmpHeader).biWidth= 512; //number of columns of the image (*pBmpHeader).biHeight= 512; //number of rows of the image (*pBmpHeader).biPlanes=1; (*pBmpHeader).biBitCount=8; (*pBmpHeader).biCompression=0; (*pBmpHeader).biSizeImage= 512*512; //raw data size = 512*512 = 26144 bytes, modify when necessary, e.g., a 256x256 image: raw data size = 256*256 (*pBmpHeader).biXPelsPerMeter=2834; (*pBmpHeader).biYpelsPerMeter=2834; (*pBmpHeader).biClrUsed=0; (*pBmpHeader).biClrImportant=0; //read provided colormap FILE *colormap=NULL; pcColorMap = new char [1024]; if((colormap=fopen("colormap.bin", "rb")) == NULL){ printf("Failed to find colormap.bin\n"); exit(0); } fread(pcColorMap, sizeof(char), pBmpHeader->bfOffBits-64, colormap); fclose(colormap); //read raw data fron input pucImageData = ReadImage( Input, pBmpHeader->biWidth, 0); printf("successfully read raw data from %s.\n\n", Input); } else if(file_answer == '2'){ //read bmp file system("cls"); //read input header infomation pBmpHeader=ReadBmpHeader(Input); printf("raw data size: %d\n", pBmpHeader->biSizeImage); printf("Image Width: %d\n", pBmpHeader->biWidth); printf("Image Height %d\n", pBmpHeader->biHeight); printf("Header occupies 54 bytes\n"); printf("Color map occupies 1024 bytes\n"); //read input colormap pcColorMap=ReadColorMap(Input, 1024); //read raw data from input //set offset=1024+54 pucImageData=ReadImage(Input, pBmpHeader->biWidth, 1078); printf("successfully read raw data from %s.\n\n", Input); } srand(time(NULL));//plant random seed //function menu char fx_answer; int width, cutoff; do{ printf( "Select function:\n" "A.Turn Lena upside down\n" "B.Turn Lena around\n" "C.Rotate Lena by 45 deg clockwise\n" "D.Shrink Lena by half\n" "E.Invert Lena\n" "F.Add normal noise to Lena\n" "G.Add impluse noise to Lena\n" "H.Moving average filtering\n" "I.Midian filtering\n" "J.Differential flitering\n" "K.LPF\n" "L.HPF\n" "\n0.Exit\n"); printf("Your choice: [_]\b\b"); fx_answer = getchar(); fx_answer = tolower(fx_answer); fflush(stdin); switch(fx_answer){//savefile(): ask user to save as picture or not case 'a':// selected: turn lena upside down UpsideDown(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'b'://selected: turn lena around LeftRight(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'c'://selected: rotate lena ImgRotate(pucImageData, pBmpHeader->biWidth, 45); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'd'://selected: shrink lena Shrink(pucImageData, pBmpHeader->biWidth, pBmpHeader, 2); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'e'://selected: invert lena Invert(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'f'://selected: add noise NormalNoise(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'g'://selected: add paper n salt noise ImpluseNoise(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'h'://selected: moving average filter //ask user to input sampling width printf("Enter sampling width:"); scanf("%d", &width); fflush(stdin); MAF(pucImageData, pBmpHeader->biWidth, width); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'i'://selected: moving midian filter //ask user to input sampling width printf("Enter sampling width:"); scanf("%d", &width); fflush(stdin); MF(pucImageData, pBmpHeader->biWidth, width); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'j'://selected: differential filter DIF(pucImageData, pBmpHeader->biWidth); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'k'://selected: low-pass filter //ask user to input cutoff frequency printf("Enter cutoff frenquency(0~%d):", (int)pBmpHeader->biWidth/2); scanf("%d", &cutoff); fflush(stdin); LPF(pucImageData, pBmpHeader->biWidth, cutoff); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case 'l'://selected: high-pass filter //ask user to input cutoff frequency printf("Enter cutoff frenquency(0~%d):", (int)pBmpHeader->biWidth/2); scanf("%d", &cutoff); fflush(stdin); HPF(pucImageData, pBmpHeader->biWidth, cutoff); savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; case '0'://selected: exit //ask one last time whether user want to save or not savefile(Output, pucImageData, file_answer, pBmpHeader, pcColorMap); break; default: system("cls"); printf("Your choice is not in the list!\n"); break; } if(fx_answer == 0) break;//selected: exit. break the loop } while(fx_answer != '0'); //returning dynamic allocated memory delete [] pcColorMap; delete [] pucImageData; delete pBmpHeader; pcColorMap=NULL; pucImageData=NULL; pBmpHeader=NULL; printf("EOP\n"); }
void pidControl() { angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/FREQ); gyroFiltered = LPF(gyroFiltered, imu.gyro.y); //kalman_filter(imu.euler.pitch, imu.gyro.y/131.0, &angleFiltered, &gyroFiltered); #if 0 balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset; balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered); currendSpeed = (currendSpeed + balanceControl.PwmLeft * 0.004) * 0.999; balanceControl.PwmLeft += currendSpeed; #endif #if 0 balanceControl.pidPitch.desired = pidUpdate(&balanceControl.pidSpeed, balanceControl.PwmLeft) + angleOffset; balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, angleFiltered); #endif #if 0 //Kp:5 Kd:1 offset:-3 20/10/-6 0.5/0.2/06 balanceControl.PwmLeft = -(1000*(balanceControl.pidPitch.Kp * ((angleFiltered + angleOffset) /90)) + balanceControl.Kd * imu.gyro.y); #endif #if 0 currendSpeed *= 0.7; currendSpeed = currendSpeed + balanceControl.PwmLeft * 0.3; position += currendSpeed; position -= speed_need; if(position<-6000000) position = -6000000; if(position> 6000000) position = 6000000; balanceControl.PwmLeft = balanceControl.pidPitch.Kp * (angleOffset - angleFiltered) +balanceControl.Kd * gyroFiltered -balanceControl.pidSpeed.Ki * position -balanceControl.pidSpeed.Kd * currendSpeed; balanceControl.PwmLeft = -balanceControl.PwmLeft; if(balanceControl.PwmLeft<-60000) balanceControl.PwmLeft = -60000; if(balanceControl.PwmLeft> 60000) balanceControl.PwmLeft = 60000; if(balanceControl.PwmLeft > - 100 && balanceControl.PwmLeft < 100) { balanceControl.PwmLeft = 0; } balanceControl.PwmRight = balanceControl.PwmLeft; #endif #if 0 float gap = abs(balanceControl.pidPitch.desired - imu.euler.pitch); if(gap > 150) { if(flag == 0) { flag = 1; balanceControl.pidPitch.Kp *= 10; balanceControl.pidPitch.Ki *= 10; balanceControl.pidPitch.Kd *= 10; //balanceControl.Kd *= 2; } } else { if(flag == 1) { flag = 0; balanceControl.pidPitch.Kp /= 10; balanceControl.pidPitch.Ki /= 10; balanceControl.pidPitch.Kd /= 10; //balanceControl.Kd /= 2; } } #endif #if 0 //works balanceControl.speed = balanceControl.speed * 0.05 + pidUpdate(&balanceControl.pidSpeed, balanceControl.speed) * 0.95; gyroFiltered = 0.05 * imu.gyro.y + gyroFiltered * 0.95; //angleFiltered = 0.2 * imu.euler.pitch + angleFiltered * 0.8; //angleFiltered = kalmanPitch.getAngle(imu.euler.pitch, imu.gyro.y/131.0, (double)1/250); //angleFiltered = kalman(imu.euler.pitch, imu.gyro.y, (double)1/FREQ); balanceControl.pidPitch.desired = balanceControl.speed + angleOffset; balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; //balanceControl.speed += pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; //balanceControl.PwmLeft = pidUpdate(&balanceControl.pidPitch, imu.euler.pitch); if(balanceControl.speed < 0.100 && balanceControl.speed > -0.100) { balanceControl.speed = 0; } //dead-band of PWM #endif #if 1 balanceControl.speed = LPF((float)(balanceControl.PwmLeft + balanceControl.PwmRight)/5600, balanceControl.speed); balanceControl.speed = pidUpdate(&balanceControl.pidSpeed, balanceControl.speed); balanceControl.pidPitch.desired = balanceControl.speed + angleOffset; balanceControl.speed = pidUpdate(&balanceControl.pidPitch, angleFiltered) + gyroFiltered * balanceControl.Kd; if(balanceControl.speed < 0.050 && balanceControl.speed > -0.050) { balanceControl.speed = 0; } //dead-band of PWM #endif //printf("dev:%6.4f ", balanceControl.pidPitch.derivative); printf("AngleSet:%4.2f AngleRef:%4.2f Angle:%4.2f error:%6.4f ", angleOffset, balanceControl.pidPitch.desired, angleFiltered, balanceControl.pidPitch.error); printf("\t| Kp:%3.2f Kd:%3.3f sumerror:%6.2f", balanceControl.pidPitch.Kp, balanceControl.Kd, balanceControl.pidPitch.sumError); printf("\t| Kp:%3.2f Ki:%3.3f sumerror:%6.2f", balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.sumError); printf("\t| speedref:%6.2f speed%6.2f error:%6.2f\r\n", balanceControl.pidSpeed.desired, balanceControl.speed, balanceControl.pidSpeed.error); /* printf("\t| Kp:%4.2f Ki:%4.2f Kd:%4.2f Speed:%4.2f angle:%4.2f |\t error:%6.4f sumerror:%6.4f Iterm:%6.4f | PWM:%6.3f\r\n", balanceControl.pidSpeed.Kp, balanceControl.pidSpeed.Ki, balanceControl.pidSpeed.Kd, balanceControl.pidSpeed.desired, balanceControl.pidPitch.desired, balanceControl.pidSpeed.error, balanceControl.pidSpeed.sumError, balanceControl.pidSpeed.intergal, balanceControl.speed); */ balanceControl.PwmLeft = 2800 * (balanceControl.speed * balanceControl.factorL - balanceControl.spinSpeed); balanceControl.PwmRight = 2800 * (balanceControl.speed * balanceControl.factorR + balanceControl.spinSpeed); }