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);
}
