bool AUDIO_Init(void){
bool bSuccess = TRUE;
AUDIO_DEBUG(("[AUDIO] AUDIO_Init...\r\n"));
if (bSuccess)
bSuccess = aduio_RegWrite(15, 0x0000); // reset
if (bSuccess)
bSuccess = aduio_RegWrite(9, 0x0000); // inactive interface
//usleep(20*1000);
if (bSuccess)
bSuccess = aduio_RegWrite(0, 0x0017); // Left Line In: set left line in volume
if (bSuccess)
bSuccess = aduio_RegWrite(1, 0x0017); // Right Line In: set right line in volume
if (bSuccess)
bSuccess = aduio_RegWrite(2, 0x005B); // Left Headphone Out: set left line out volume
if (bSuccess)
bSuccess = aduio_RegWrite(3, 0x005B); // Right Headphone Out: set right line out volume
if (bSuccess) // enable mic-boost, bypass and sidetone
bSuccess = aduio_RegWrite(4, 0x0015 | 0x20 | 0x08 | 0x01); // Analogue Audio Path Control: set mic as input and boost it, and enable dac
// bSuccess = aduio_RegWrite(4, 0x0015); // Analogue Audio Path Control: set mic as input and boost it, and enable dac
if (bSuccess)
bSuccess = aduio_RegWrite(5, 0x0000); // Digital Audio Path Control: disable soft mute
if (bSuccess)
bSuccess = aduio_RegWrite(6, 0); // power down control: power on all
if (bSuccess)
bSuccess = aduio_RegWrite(7, 0x0042); // I2S, iwl=16-bits, Enable Master Mode
// bSuccess = aduio_RegWrite(7, 0x0041); // MSB-First, left justified, iwl=16-bits, Enable Master Mode
if (bSuccess)
bSuccess = aduio_RegWrite(8, 0x0002); // Normal, Base OVer-Sampleing Rate 384 fs (BOSR=1)
if (bSuccess)
bSuccess = aduio_RegWrite(9, 0x0001); // active interface
AUDIO_DEBUG(("[AUDIO] AUDIO_Init %s\r\n", bSuccess?"success":"fail"));
return bSuccess;
}
bool aduio_RegWrite(alt_u8 reg_index, alt_u16 data16){
bool bSuccess;
alt_u8 data, control;
if (reg_index <= 10)
reg_file[reg_index] = data16;
data = data16 & 0xFF;
control = (reg_index << 1) & 0xFE;
control |= ((data16 >> 8) & 0x01);
AUDIO_DEBUG(("[AUDIO] set audio reg[%02d] = %04Xh\r\n", reg_index, data16));
bSuccess = I2C_Write(I2C_SCL_BASE, I2C_SDA_BASE, I2C_AUDIO_ADDR, control, data);
if (!bSuccess)
AUDIO_DEBUG(("[AUDIO] write reg fail!!!!\r\n"));
usleep(50*1000); // wait audio chip read
return bSuccess;
}
/****************************************************************************
NAME
audio_switch_voip_music_mode - Switch the audio mode in use (VOIP \ MUSIC)
*/
void audio_switch_voip_music_mode(AUDIO_VOIP_MUSIC_MODE_T new_mode)
{
if (states_get_state() == SOURCE_STATE_CONNECTED)
{
AUDIO_DEBUG(("AUDIO: Switch Audio Mode\n"));
if ((new_mode == AUDIO_VOIP_MODE) &&
(theSource->audio_data.audio_voip_music_mode == AUDIO_MUSIC_MODE))
{
if (AGHFP_PROFILE_IS_ENABLED && aghfp_get_number_connections())
{
/* only switch if bidirectional support is not enabled, otherwise we're
* running with a codec and a remote device which is already feeding us
* a MIC back channel */
if (!theSource->audio_data.audio_remote_bidir_support)
{
/* switch to VOIP mode from MUSIC mode */
audio_set_voip_music_mode(AUDIO_VOIP_MODE);
MessageSend(&theSource->app_data.appTask, APP_AUDIO_START, 0);
}
}
}
else if ((new_mode == AUDIO_MUSIC_MODE) &&
(theSource->audio_data.audio_voip_music_mode == AUDIO_VOIP_MODE))
{
if (A2DP_PROFILE_IS_ENABLED && a2dp_get_number_connections())
{
/* switch to MUSIC mode from VOIP mode */
audio_set_voip_music_mode(AUDIO_MUSIC_MODE);
MessageSend(&theSource->app_data.appTask, APP_AUDIO_START, 0);
/* make sure Voice Recognition is disabled as this can cause audio routing issues on the remote device */
aghfp_send_voice_recognition(FALSE);
}
}
}
}
/****************************************************************************
NAME
audio_a2dp_update_bitpool - Change the bitpool for the A2DP audio
*/
void audio_a2dp_update_bitpool(uint8 bitpool, uint8 bad_link_bitpool)
{
if (theSource->audio_data.audio_routed == AUDIO_ROUTED_A2DP)
{
/* change A2DP SBC bitpools */
theSource->audio_data.audio_a2dp_mode_params.bitpool = bitpool;
theSource->audio_data.audio_a2dp_mode_params.bad_link_bitpool = bad_link_bitpool;
AUDIO_DEBUG(("AUDIO: audio_a2dp_update_bitpool - bitpool[%d] bad_link_bitpool[%d]\n", bitpool, bad_link_bitpool));
audio_update_mode_parameters();
}
}
/****************************************************************************
NAME
audio_aghfp_connect - Route AGHFP audio
*/
void audio_aghfp_connect(Sink sink, bool esco, bool wbs, uint16 size_warp, uint16 *warp)
{
uint16 i = 0;
AudioPluginFeatures features = {0,0,0}; /* no stereo or i2s output */
AUDIO_DEBUG(("AUDIO: audio_aghfp_connect\n"));
/* start audio active timer */
audio_start_active_timer();
/* remove any A2DP audio */
audio_a2dp_disconnect_all();
theSource->audio_data.audio_aghfp_connect_params.mic = NULL;
theSource->audio_data.ag_usb_params.usb_source = usb_get_speaker_source(); /* Set the USB Source */
theSource->audio_data.ag_usb_params.usb_sink = usb_get_mic_sink(); /* Set the USB Sink */
theSource->audio_data.audio_aghfp_connect_params.usb = &theSource->audio_data.ag_usb_params;
for (i = 0; i < size_warp; i++)
{
theSource->audio_data.audio_aghfp_connect_params.warp[i] = warp[i];
}
if (theSource->audio_data.audio_routed == AUDIO_ROUTED_NONE)
{
theSource->audio_data.audio_routed = AUDIO_ROUTED_AGHFP;
AudioConnect(audio_aghfp_get_plugin(wbs),
sink,
esco ? AUDIO_SINK_ESCO : AUDIO_SINK_SCO,
theSource->codec,
/*aghfp_gain*/10,
8000,
features , /* no stereo or I2S output required */
volume_get_mute_mode(),
AUDIO_ROUTE_INTERNAL,
/*power*/0,
&theSource->audio_data.audio_aghfp_connect_params,
&theSource->audioTask);
}
}
bool AUDIO_SetLineInVol(alt_u16 l_vol, alt_u16 r_vol){
bool bSuccess;
alt_u16 control;
// left
control = reg_file[0];
control &= 0xFFE0;
control += l_vol & 0x1F;
bSuccess = aduio_RegWrite(0, control);
if (bSuccess){
// right
control = reg_file[1];
control &= 0xFFE0;
control += r_vol & 0x1F;
bSuccess = aduio_RegWrite(1, control);
}
AUDIO_DEBUG(("[AUDIO] set Line-In vol(%d,%d) %s\r\n", l_vol, r_vol, bSuccess?"success":"fail"));
return bSuccess;
}
/****************************************************************************
NAME
audio_a2dp_disconnect - Disconnect A2DP audio
*/
void audio_a2dp_disconnect(uint16 device_id, Sink media_sink)
{
uint16 index = 0;
bool active_audio = FALSE;
if (media_sink)
{
if (theSource->audio_data.audio_routed == AUDIO_ROUTED_A2DP)
{
/* store that this media is now disconnected */
theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[device_id] = 0;
/* see if any other A2DP media is active */
for (index = 0; index < CSR_A2DP_ENCODER_PLUGIN_MAX_A2DP_SINKS; index++)
{
if (theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[index])
{
active_audio = TRUE;
}
}
if (active_audio)
{
/* A2DP media still active so just disconnect one of the A2DP audio streams */
theSource->audio_data.audio_a2dp_mode_params.disconnect_sink = media_sink;
AUDIO_DEBUG((" Disconnect A2DP Audio: sink [0x%x]\n",
(uint16)media_sink));
audio_update_mode_parameters();
}
else
{
/* no A2DP media still active so disconnect all A2DP audio streams */
audio_a2dp_disconnect_all();
}
}
}
}
bool AUDIO_SetLineOutVol(alt_u16 l_vol, alt_u16 r_vol){
bool bSuccess;
alt_u16 control;
// left
control = reg_file[2];
control &= 0xFF80;
control += l_vol & 0x7F;
bSuccess = aduio_RegWrite(2, control);
if (bSuccess){
// right
control = reg_file[3];
control &= 0xFF80;
control += r_vol & 0x7F;
bSuccess = aduio_RegWrite(3, control);
}
AUDIO_DEBUG(("[AUDIO] set Line-Out vol(%d,%d) %s\r\n", l_vol, r_vol, bSuccess?"success":"fail"));
return bSuccess;
}
/****************************************************************************
NAME
audio_a2dp_set_plugin - Set A2DP audio plugin
*/
void audio_a2dp_set_plugin(uint8 seid)
{
AUDIO_DEBUG(("AUDIO: audio_a2dp_set_plugin [%d]\n", seid));
switch (seid)
{
case A2DP_SEID_SBC:
{
theSource->audio_data.audio_plugin = (TaskData *)&csr_a2dp_sbc_encoder_plugin;
}
break;
case A2DP_SEID_FASTSTREAM:
{
theSource->audio_data.audio_plugin = (TaskData *)&csr_a2dp_faststream_encoder_plugin;
}
break;
case A2DP_SEID_APTX:
{
theSource->audio_data.audio_plugin = (TaskData *)&csr_a2dp_aptx_encoder_plugin;
}
break;
case A2DP_SEID_APTX_LOW_LATENCY:
{
theSource->audio_data.audio_plugin = (TaskData *)&csr_a2dp_aptx_lowlatency_encoder_plugin;
}
break;
default:
{
}
break;
}
}
task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool sawRedBlue = false;
bool goBackward = false;
float startAngle = 0;
//waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(state.color2 == RED || state.color2 == BLUE){
sawRedBlue = true;
}
if (sawRedBlue && state.color2 == BLACK){
state.currentState = LINEFOLLOW;
}
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
/* state FINDLINE_DRIVE seems unnecessary
} else if (state.currentState == FINDLINE_DRIVE) {
drive(DRIVESPEED, DRIVESPEED*.95);
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if(state.color2 == RED || state.color2 == BLUE){
state.currentState = LINEFOLLOW;
} */
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true) {
state.currentState = GOTOEND;
} else if (goBackward){
if (state.color == RED || state.color == BLUE) {
leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = -DRIVESPEED;
}else {
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
}
} else {
if (state.color2 == RED || state.color2 == BLUE) {
leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;
}
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if(state.color == RED || state.color == BLUE){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 22.5){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && state.x_accel < 100){
wait1Msec(20);
state.currentState = CHECKEND;
}
} else if (state.currentState == CHECKEND) {
if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){
state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done
} else {
state.currentState = PARK_DRIVE2; //else you need to do more pushing
}
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 1000){
break;
}
} else {
break;
}
}
}
task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool goBackward = false;
float startAngle = 0;
int numTimeAccelTriggered = 0;
waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true){
state.currentState = GOTOEND;
} else if (goBackward){
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED;
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if((state.color == RED || state.color == BLUE) && time1[T1] > 500){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 27){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && time1[T1] >= 300){
numTimeAccelTriggered++;
} else {
numTimeAccelTriggered = 0;
}
if (numTimeAccelTriggered >= 3) {
state.currentState = END;
}
wait1Msec(20);
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 300){
break;
}
} else {
break;
}
}
}
/****************************************************************************
NAME
audio_set_voip_music_mode
DESCRIPTION
Set the audio mode in use (VOIP \ MUSIC).
*/
void audio_set_voip_music_mode(AUDIO_VOIP_MUSIC_MODE_T mode)
{
AUDIO_DEBUG(("AUDIO: Audio Mode [%d]\n", mode));
theSource->audio_data.audio_voip_music_mode = mode;
}
/****************************************************************************
NAME
audio_a2dp_connect - Route A2DP audio
*/
void audio_a2dp_connect(Sink sink, uint16 device_id, uint16 stream_id)
{
uint8 bitpool;
uint8 bad_link_bitpool;
bool multiple_streams;
/* start audio active timer */
audio_start_active_timer();
/* remove any AGHFP audio */
audio_aghfp_disconnect();
AUDIO_DEBUG(("AUDIO: audio_a2dp_connect\n"));
if (theSource->audio_data.audio_routed == AUDIO_ROUTED_NONE)
{
theSource->audio_data.audio_a2dp_connect_params.input_source = usb_get_speaker_source(); /* Set the USB Source, not used for Analogue */
theSource->audio_data.audio_a2dp_connect_params.input_sink = usb_get_mic_sink(); /* Set the USB Sink, not used for Analogue */
theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[device_id] = sink; /* Set the A2DP media Sink */
AUDIO_DEBUG((" audio_routed [%d] input_source [0x%x] input_sink [0x%x] a2dp_sink_0 [0x%x] a2dp_sink_1 [0x%x]\n",
theSource->audio_data.audio_routed,
(uint16)theSource->audio_data.audio_a2dp_connect_params.input_source,
(uint16)theSource->audio_data.audio_a2dp_connect_params.input_sink,
(uint16)theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[0],
(uint16)theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[1]));
if (theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[0] || theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[1])
{
AudioPluginFeatures features = {0,0,0}; /* no stereo or i2s output */
a2dp_codec_settings *codec_settings = A2dpCodecGetSettings(device_id, stream_id);
if (codec_settings)
{
AUDIO_DEBUG((" codec ; voice_rate[0x%lx] packet_size[0x%x] bitpool[0x%x] format[0x%x] CP[0x%x]\n",
codec_settings->codecData.voice_rate,
codec_settings->codecData.packet_size,
codec_settings->codecData.bitpool,
codec_settings->codecData.format,
codec_settings->codecData.content_protection
));
theSource->audio_data.audio_a2dp_connect_params.rate = codec_settings->codecData.voice_rate;
theSource->audio_data.audio_a2dp_connect_params.packet_size = codec_settings->codecData.packet_size;
if (a2dp_get_sbc_bitpool(&bitpool, &bad_link_bitpool, &multiple_streams))
{
theSource->audio_data.audio_a2dp_connect_params.bitpool = bitpool;
theSource->audio_data.audio_a2dp_connect_params.bad_link_bitpool = bad_link_bitpool;
}
else
{
theSource->audio_data.audio_a2dp_connect_params.bitpool = codec_settings->codecData.bitpool;
}
theSource->audio_data.audio_a2dp_connect_params.format = codec_settings->codecData.format;
theSource->audio_data.audio_a2dp_mode_params.eq_mode = theSource->volume_data.eq_index;
theSource->audio_data.audio_a2dp_connect_params.mode = &theSource->audio_data.audio_a2dp_mode_params;
/* turn on content protection if negotiated */
theSource->audio_data.audio_a2dp_connect_params.content_protection = codec_settings->codecData.content_protection;
/* remember if the remote device supports bidirectional audio, i.e. has a MIC back channel */
theSource->audio_data.audio_remote_bidir_support = audio_is_bidir_supported(codec_settings) ? 1 : 0;
theSource->audio_data.audio_routed = AUDIO_ROUTED_A2DP;
audio_a2dp_get_plugin();
AudioConnect(audio_a2dp_get_plugin(),
0,
AUDIO_SINK_AV,
theSource->codec,
/*a2dp_gain*/10,
codec_settings->rate,
features, /* stereo supported, no i2s output */
volume_get_mute_mode(),
AUDIO_ROUTE_INTERNAL,
/*power*/0,
&theSource->audio_data.audio_a2dp_connect_params,
&theSource->audioTask);
/* free the codec_settings memory that the A2DP library allocated */
memory_free(codec_settings);
}
}
}
else if (theSource->audio_data.audio_routed == AUDIO_ROUTED_A2DP)
{
/* connecting additional A2DP device */
theSource->audio_data.audio_a2dp_connect_params.a2dp_sink[device_id] = sink;
theSource->audio_data.audio_a2dp_mode_params.connect_sink = sink;
if (a2dp_get_sbc_bitpool(&bitpool, &bad_link_bitpool, &multiple_streams))
{
theSource->audio_data.audio_a2dp_mode_params.bitpool = bitpool;
theSource->audio_data.audio_a2dp_mode_params.bad_link_bitpool = bad_link_bitpool;
}
AUDIO_DEBUG((" audio_routed [%d] connect_sink [0x%x] bitpool [%d] bad_link_bitpool [%d]\n",
theSource->audio_data.audio_routed,
(uint16)sink,
bitpool,
bad_link_bitpool));
audio_update_mode_parameters();
}
}