int
PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
{
// Clamp value between 0 and 4095 inclusive.
if (val > 4095) {
val = 4095;
}
if (invert) {
if (val == 0) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 4095) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, 4095 - val);
}
} else {
if (val == 4095) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 0) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, val);
}
}
return PX4_ERROR;
}
int main(void){
TRISE = TRISE | 0xF0;
//adc
TRISBbits.TRISB14=1;//rb14 ativo input
AD1PCFGbits.PCFG14=0;//rb14 analog
AD1CON1bits.SSRC = 7;
AD1CON1bits.CLRASAM = 1;
AD1CON3bits.SAMC = 16;
AD1CON2bits.SMPI = Namostras-1;
AD1CHSbits.CH0SA = 14;
AD1CON1bits.ON = 1;
//configure interup
IPC6bits.AD1IP=1;
IEC1bits.AD1IE=1;
//configure interuo of T1
T1CONbits.TCKPS = 3; // 1:256 prescaler (i.e fin = 625 KHz)
PR1 = PBCLK/256/4-1; // Fout = 20MHz / (256 * (78125 + 1)) = 4 Hz
TMR1 = 0; // Reset timer T2 count register
T1CONbits.TON = 1; // Enable timer T2 (must be the last command of the timer configuration sequence)
IFS0bits.T1IF = 0; // Reset timer T2 interrupt flag
IPC1bits.T1IP = 2; // Interrupt priority (must be in range [1..6])
IEC0bits.T1IE = 1; // Enable timer T2 interrupts
//configure interup of T3
T3CONbits.TCKPS = 2; // 1:4 prescaler
PR3 = PBCLK/4/200-1; // Fout = 20MHz / (2 * (62499 + 1)) = 100 Hz
TMR3 = 0; // Reset timer T2 count register
T3CONbits.TON = 1; // Enable timer T2 (must be the last command of the timer configuration sequence)
IFS0bits.T3IF = 0; // Reset timer T2 interrupt flag
IPC3bits.T3IP = 1; // Interrupt priority (must be in range [1..6])
IEC0bits.T3IE = 1; // Enable timer T2 interrupts
OC1CONbits.OCM = 6; // PWM mode on OCx; fault pin disabled
OC1CONbits.OCTSEL=1; // Use timer T2 as the time base for PWM generation
setPWM(0); // Ton constant
OC1CONbits.ON = 1; // Enable OC1 module
IFS1bits.AD1IF = 0; // Reset AD1IF flag
EnableInterrupts();
while(1){
char readSwitch = PORTE & 0x00F0;
readSwitch = readSwitch >> 4;
char op1 = readSwitch & 0x0003;
char op2 = (readSwitch >> 2) & 0x0003;
printf("OP1 -> %d OP2 -> %d\r ", op1, op2);
switch(op1){
case 0: // 00 – funciona como voltímetro (o LED deve ficar OFF)
IEC0bits.T1IE = 1; // Enable timer T1 interrupts
setPWM(0);
//printf("case 0\n");
break;
case 1: //01 – congela o valor da tensão (o LED fica ON com o brilho no máximo)
IEC0bits.T1IE = 0; // Desenable timer T1 interrupts
setPWM(100);
//printf("case 1\n");
break;
case 2: // 10 - controlo do brilho do LED (dependente dos bits RE7 e RE6)
//printf("case 2\n");
setPWM(pwmValues[(int)op2]);
value2display=pwmValues[(int)op2];
break;
default:
break;
}
}
return 0;
}
void turnOffLedAll(void) {
_ledOff = true;
setPWM(0);
turnOffLED(LEDFULLLED);
}
char execCommand(InPackStruct* cmd) //обработать входящую команду
{
switch(cmd->command)
{
case 0x01: //Эхо
{
char *key= cmd->param;
if ((key[0] =='E')&&(key[1] =='C')&&(key[2] =='H')&&(key[3] =='O') )
{
char * str ="mobile robot V1.0";
sendAnswer(cmd->command, str, strlen(str)+1);
}
}
break;
case 0x02: //Установить текущие координаты
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
points[0].center[0]= temp[0];
points[0].center[1]= temp[1];
points[0].center[2]= temp[2];
CreatePath(&points[0], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x03: //установить скважность шим
{
char ch = *cmd->param;
float temp =*((float*)(cmd->param + 1));
setPWM( ch - 1, temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x04: //Установить бит направления
{
char * ch = cmd->param;
set_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x05: //Снять бит направления
{
char * ch = cmd->param;
reset_pin(PWM_DIR[(*ch)-1]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x06: //Установить напряжение на двигателе
{
char ch = *cmd->param;
float duty = *((float*)(cmd->param + 1));
uint16_t dir = *((uint16_t*)(cmd->param + 2));
setVoltageMaxon( ch-1, dir, duty);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x08: //Установить параметры регулятора
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=3; i++)
{
wheelsPidStruct[i].p_k = temp[0];
wheelsPidStruct[i].i_k = temp[1];
wheelsPidStruct[i].d_k = temp[2];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x09: //Установить требуюему скорость двигателей
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i <= 3; i++)
{
regulatorOut[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0B: //Включить рассчет кинематики
{
curState.kinemEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0C: //Выключить рассчет кинематики
{
curState.kinemEn = 0;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0D: //Задать скорости движения
{
float *(temp) ={(float*)cmd->param};
char i;
for (i = 0; i<=2; i++)
{
vTargetGlob[i] = temp[i];
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0E: //Включить траекторный регулятор
{
curState.trackEn=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x0F: //Выключить траекторный регулятор
{
curState.trackEn=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x10: //Очистить очередь точек
{
while(lastPoint>0) removePoint(&points[0],&lastPoint);
points[0].center[0]= robotCoord[0];
points[0].center[1]= robotCoord[1];
points[0].center[2]= robotCoord[2];
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x11: //Добавить точку в очередь
{
float *(temp) ={(float*)(cmd->param)};
char * ch = cmd->param + 12;
lastPoint++;
points[lastPoint].center[0] = temp[0];
points[lastPoint].center[1] = temp[1];
points[lastPoint].center[2] = temp[2];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x12: //Состояние очереди точек
{
char outdata[15];
float * temp =(float*)(&outdata[3]);
char * cntPoint = (&outdata[0]);
uint16_t * curPoint = (uint16_t *)(&outdata[1]);
*cntPoint= lastPoint;
*curPoint = totalPointComplite;
temp[0]= points[0].center[0];
temp[1]= points[0].center[1];
temp[2]= points[0].center[2];
//char * str ="Ok";
sendAnswer(cmd->command,outdata, 15);
}
break;
case 0x13: //отправить текущие координаты
{
sendAnswer(cmd->command,(char *)robotCoord, sizeof(robotCoord));
}
break;
case 0x14: //отправить текущую скорость
{
sendAnswer(cmd->command,(char *)robotSpeed, sizeof(robotCoord));
}
break;
case 0x15: //Задать скорость движения
{
float *(temp) = {(float*)(cmd->param)};
char i;
for (i = 0; i<=4; i++)
normalVelFast[i]= temp[i];
for (i = 0; i<=4; i++)
stopVelFast[i]= temp[i];
stopVelFast[2]=-0.2;
char * str = "Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x16: //Установить режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
pinType[ch] = *((char *)(cmd->param +1));
if (pinType[ch] == ADC_ANALOG_PIN )
conf_pin(GENERAL_PIN[ch], ANALOG, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_INPUT )
conf_pin(GENERAL_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
if (pinType[ch] == ADC_DIG_OUTPUT )
conf_pin(GENERAL_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x17: //отправить состояние выбранного входа АЦП
{
char ch = (*((char *)(cmd->param))) - 1;
if (ch < 10)
sendAnswer(cmd->command,(char *)&(adcData[ch]), sizeof(uint16_t));
}
break;
case 0x18: //отправить состояние всех АЦП
{
sendAnswer(cmd->command,(char *)adcData, sizeof(adcData));
}
break;
case 0x19: //отправить состояние входа
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
char temp = (pin_val(GENERAL_PIN[ch])!=0);
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1A: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(GENERAL_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x1B: //установить состояние выхода
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(GENERAL_PIN[ch]); else
set_pin(GENERAL_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x1C: //отправить текущий режим ножки
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(pinType[ch]), sizeof(uint8_t));
}
break;
case 0x1D: //установить режим ножки EXTI
{
char ch = (*((char *)(cmd->param))) -1;
if (ch<10)
{
extiType[ch] = (*((char *)(cmd->param +1)));
if (extiType[ch] == EXTI_BOTH )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_BOTH_EDGES);
}
if (extiType[ch] == EXTI_RISE )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_RISING_EDGE);
}
if (extiType[ch] == EXTI_FALL )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, PULL_UP);
add_ext_interrupt(EXTI_PIN[ch], EXTI_FALLING_EDGE) ;
}
if (extiType[ch] == EXTI_DIG_INPUT )
{
conf_pin(EXTI_PIN[ch], INPUT, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
if (extiType[ch] == EXTI_DIG_OUTPUT )
{
conf_pin(EXTI_PIN[ch], GENERAL, PUSH_PULL, FAST_S, NO_PULL_UP);
clear_ext_interrupt(EXTI_PIN[ch]) ;
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x1E: //отправить состояние входа
{
char ch = *((char *)(cmd->param))-1;
if ((ch)<10)
{
char temp = ch;
if (pin_val(EXTI_PIN[ch])) temp |=0x80;
sendAnswer(cmd->command,&temp, sizeof(temp));
}
}
break;
case 0x1F: //отправить состояние всех входов
{
char temp[10];
char i ;
for ( i = 0; i<10; i++) temp[i] = (pin_val(EXTI_PIN[i])!=0);
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x20: //установить состояние выхода
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
if (*(cmd->param+1)==0) reset_pin(EXTI_PIN[ch]); else
set_pin(EXTI_PIN[ch]);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x21: //отправить текущий режим ножки
{
char ch = *((char *)(cmd->param))-1;
if (ch<10)
sendAnswer(cmd->command,(char *) &(extiType[ch]), sizeof(uint8_t));
}
break;
case 0x22: //установить состояние выхода +12В
{
char ch = (*((char *)(cmd->param)))-1;
if (ch<6)
{
if (*(cmd->param+1)==0)
reset_pin(V12_PIN[ch]); else
set_pin(V12_PIN[ch]);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x23: //Выключить ПИД регуляторы приводов
{
curState.pidEnabled=0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x24: //Включить ПИД регуляторы приводов
{
curState.pidEnabled=1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x25: //set current coordinate
{
float *(temp) ={(float*)cmd->param};
robotCoord[0]= temp[0];
robotCoord[1]= temp[1];
robotCoord[2]= temp[2];
addPointInFrontOfQueue(&points[0], &temp[0], (char) 4, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
case 0x26: //set dynamixel angle
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(angle) ={(uint16_t*)(cmd->param + 1)};
if (setServoMovingSpeed(ID, angle))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x27: //set CW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x28: //set CCW angle limit
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(limit) ={(uint16_t*)(cmd->param + 1)};
if (setServoCCWAngleLimit(ID, limit))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x29: //set servo moving speed
{
uint8_t *(ID) ={(uint8_t*)cmd->param};
uint16_t *(speed) ={(uint16_t*)(cmd->param + 1)};
uint16_t *(direction) ={(uint16_t*)(cmd->param + 3)};
if (setServoMovingSpeed(ID, speed, direction))
{
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
}
break;
case 0x2A: //add a point to the beginning of Queue
{
float *(temp) = (float*)(cmd->param);
char * ch = cmd->param + 12;
addPointInFrontOfQueue(&points[0], &temp[0], &ch, &lastPoint);
CreatePath(&points[1], &points[0], &curPath);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2B: //Open Cubes Catcher
{
openCubesCatcher();
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x2C: //Close Cubes Catcher
{
uint8_t numberOfCubesCatched;
closeCubesCatcher(&numberOfCubesCatched);
sendAnswer(cmd->command, (char *)&numberOfCubesCatched, sizeof(uint8_t));
}
break;
case 0x2D: // Open wall
{
openWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2E: // Close wall
{
closeWall();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x2F: // Switch On the vibration
{
uint8_t temp ={*(uint8_t*)(cmd->param)};
switchOnVibration(temp);
char * str ="Ok";
sendAnswer(cmd->command, str, 3);
}
break;
case 0x30: // Switch Off the vibration
{
switchOffVibration();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x31: // Set angle of cubes catcher
{
float *(temp) = (float*)(cmd->param);
cubesCatcherPID.target = *temp;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x32: // Flag of reached point
{
sendAnswer(cmd->command, (char *)&traceFlag, sizeof(traceFlag));
}
break;
case 0x33: // Switch on collision avoidance
{
curState.collisionAvoidance = 1;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x34: // Switch off collision avoidance
{
curState.collisionAvoidance = 0;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x35: // Get Cubes Catcher angle
{
sendAnswer(cmd->command, (char *)&cubesCatcherPID.current, sizeof(cubesCatcherPID.current));
}
break;
case 0x36: // Get IR Distances
{
float temp[4];
int i = 0;
for(; i < 5; i++)
{
temp[i] = distanceFromIR[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x37: // Get Sonar Distances
{
float temp[5];
int i = 0;
for(; i < 6; i++)
{
temp[i] = distanceFromSonars[i][0];
}
sendAnswer(cmd->command,(char *)temp, sizeof(temp));
}
break;
case 0x38: // Stop command
{
closeWall();
openCubesCatcher();
curState.pidEnabled = 0;
switchOffVibration();
char i;
for (i = 0; i < 4; i++)
{
setVoltageMaxon(WHEELS[i], (uint8_t) 1, (float) 0);
}
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x39: // Generate new trajectory with correction
{
float *(temp) ={(float*)cmd->param};
char * ch = cmd->param + 24;
robotCoord[0] = temp[0];
robotCoord[1] = temp[1];
robotCoord[2] = temp[2];
lastPoint++;
points[lastPoint].center[0] = temp[3];
points[lastPoint].center[1] = temp[4];
points[lastPoint].center[2] = temp[5];
points[lastPoint].speedVelTipe = speedType[*ch];
points[lastPoint].speedRotTipe = rotType[*ch];
points[lastPoint].endTask = NULL;
points[lastPoint].movTask = NULL;
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3A: // Open cubes catcher widely
{
openCubesCatcherWidely();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3B : // Kick the cone on a purple side
{
moveCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
case 0x3C: // Close the cone kicker cone on a purple side
{
closeCone();
char * str ="Ok";
sendAnswer(cmd->command,str, 3);
}
break;
default:
break;
}
return 0;
}
int8_t DigitalOutput::showWebinterface(WebServer* server,
WebServer::ConnectionType type,
char* url)
{
#ifdef PROVIDE_WEBIF
File templateFile;
TemplateParser* parser;
int16_t matchIdx;
char templateFileName[template_digitaloutput_fnsize];
strcpy_P(templateFileName, template_digitaloutput_fname);
float dC = -1.0;
if (type == WebServer::POST)
{
int8_t repeat;
char name[16], value[16];
do
{
repeat = server->readPOSTparam(name, 16, value, 16);
if (strcmp_P(name,
(PGM_P) pgm_read_word(&(template_digitaloutput_inputs[DO_I_ON])))
== 0)
{
m_OnValue = atoi(value);
m_OffValue = 1 - m_OnValue;
}
else if (strcmp_P(name,
(PGM_P) pgm_read_word(&(template_digitaloutput_inputs[DO_I_PIN])))
== 0)
{
m_Pin = atoi(value);
pinMode(m_Pin, OUTPUT);
}
else if (strcmp_P(name,
(PGM_P) pgm_read_word(&(template_digitaloutput_inputs[DO_I_PWM])))
== 0)
{
dC = atof(value);
if (dC < 0.0)
dC = 0;
else if (dC > 100.0)
dC = 100.0;
dC /= 100;
}
} while (repeat);
if (dC >= 0)
setPWM(dC);
server->httpSeeOther(this->m_URL);
}
else
{
server->httpSuccess();
parser = __aquaduino->getTemplateParser();
templateFile = SD.open(templateFileName, FILE_READ);
while ((matchIdx =
parser->processTemplateUntilNextMatch(&templateFile,
template_digitaloutput,
template_digitaloutput_elements,
server))
>= 0)
{
switch (matchIdx)
{
case DO_ANAME:
server->print(getName());
break;
case DO_PIN_NAME:
server->print((const __FlashStringHelper *) (pgm_input_pin));
break;
case DO_PIN_VAL:
server->print(m_Pin);
break;
case DO_PIN_SIZE:
server->print(3);
break;
case DO_PIN_MAXLENGTH:
server->print(2);
break;
case DO_PWM_NAME:
server->print((const __FlashStringHelper *) (pgm_input_pwm));
break;
case DO_PWM_VAL:
if (supportsPWM())
{
server->print(getPWM() * 100);
}
else
{
server->print(F("No PWM"));
}
break;
case DO_PWM_SIZE:
server->print(7);
break;
case DO_PWM_MAXLENGTH:
server->print(6);
break;
case DO_ON_NAME:
server->print((const __FlashStringHelper *) (pgm_input_on));
break;
case DO_ON_OPTIONS:
parser->selectListOption("LOW", "0", m_OnValue == 0, server);
parser->selectListOption("HIGH", "1", m_OnValue == 1, server);
break;
}
}
templateFile.close();
}
#endif //PROVIDE_WEBIF
return true;
}
void setupCw2MotorPoewrLevel(unsigned short CW2) {
motorPowerLevel_CW2 = CW2;
setPWM(SOFT_PWM_CW2, motorPowerLevel_CW2);
}
/*!
\param led channel to set PWM value for
\param value 0-4095 value for PWM
*/
void PCA9685::setPWM(uint8_t led, int value) {
setPWM(led, 0, value);
}
/** Set channel's pulse length in microseconds
* @param Channel number (0-15)
* @param Length in microseconds
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWMuS(uint8_t channel, float length_uS) {
setPWM(channel, round((length_uS * 4096.f) / (1000000.f / frequency)));
}
static void speedOut() { //speedout
if (!MotorEnable)
return;
if (OutputLeft == 0) {
setPWM(PWM_1, 0);
setPWM(PWM_2, 0);
} else if (OutputLeft > 0) {
setPWM(PWM_1, 0);
setPWM(PWM_2, OutputLeft + MotorOffestL);
} else if (OutputLeft < 0) {
setPWM(PWM_1, 0);
setPWM(PWM_2, MotorOffestL - OutputLeft);
}
if (OutputRight == 0) {
setPWM(PWM_3, 0);
setPWM(PWM_4, 0);
} else if (OutputRight > 0) {
setPWM(PWM_3, OutputRight + MotorOffestR);
setPWM(PWM_4, 0);
} else if (OutputRight < 0) {
setPWM(PWM_3, 0);
setPWM(PWM_4, MotorOffestR - OutputRight);
}
}
//-----------------------------------------------------------------------------
// MAIN STARTS HERE -----------------------------------------------------------
int main()
{
// BEGIN
Initialise(); // Function to do initial setup
while(1) // Infinite loop
{
PORTD = 0xff; // This is just to indicate that PIC is running
// this can be change for better use of debugging
if (F.I2C == 1) // I2C message was received
{
// Perform some I2C operation
// If read, reset Data and PID information
// Perform operation for Target
Target = i2cTarget; // Receive velocity from CPU
setDirection(MOTOR_DIRECTION); // Set direction of the motor to the direction
// sent from CPU
OdometryCounts = 0; // Reset the variables for PID and odometry
if (i2cTarget == 0) // This to ensure a sharp stop
{
AccumulatedError = 0;
setPWM(0);
}
// Clear Flag
F.I2C = 0;
}
if (F.DIR == 1)
{
// Update counts before updating direction
encUpdate(&EncoderCounts); //This will put the value of TMR1 into counts and then clear TMR0
updateData(EncoderCounts); // This will add counts to OdometryCounts (which is the total distanced traveled so far.)
// Update direction
DirectionRead = PORTBbits.RB5;
// Clear Flag
F.DIR = 0;
}
// PID Loop occurs at regular time intervals
if (F.T0 == 1)
{
// Update to most recent encoder counts
encUpdate(&EncoderCounts);
updateData(EncoderCounts);
/****** I'm keeping this in here just incase I change my mind and want to implement ***
* it later on. PID is perfectly function right now
// if ((DirectionRead != MOTOR_DIRECTION)&& (Target != 0))
// {
// intSecondComplement(&EncoderCounts);
// PORTBbits.RB3 = DirectionRead;
// }
/**************************************************************************************/
/*************** Calculate stuff for PID **********************************************/
Error = Target - EncoderCounts;
AccumulatedError = AccumulatedError + Error;
DeltaError = Error - PreviousError;
PreviousError = Error;
// Set bounds for the accumulated error ///
if (AccumulatedError > 10000)
AccumulatedError = 10000;
else if (AccumulatedError < -10000)
AccumulatedError = -10000;
// Performing the actual PID
PID = (Error * KP) + (AccumulatedError * KI) + (DeltaError * KD ); // Performing PID calculation
//Note: Even through the variable types are
// different, the result should be the correct
// value of with the type of int
if (Error != 0) // If no error
{
CurrentPwm = PID + PWM_OFFSET;
if (CurrentPwm >= 255) // PWM should be between 0 - 255
CurrentPwm = 255;
else if (CurrentPwm < 0)
CurrentPwm = 0;
setPWM(CurrentPwm); // set new PWM
}
F.T0 = 0; // reset TMR0 flag
} // end PID Loop
} // end while(1)
return 1; // standard ending for an "int main"
} // END MAIN ---------------------------------------------------------------
/** Set channel's pulse length
* @param Channel number (0-15)
* @param Length (0-4095)
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWM(uint8_t channel, uint16_t length) {
setPWM(channel, 0, length);
}
void openOrClosePWM(uchar open){
PWM_CONTROL_FLAG = open;
TMR2IE = open; //关闭timer2中断
setPWM(open);
}
void PCA9685::setDgt(uint8_t led, bool value)
{
if (value) setPWM(led, 1, 0);
else setPWM(led, 1, 1);
}
void LED::updatePWM() {
setPWM(CHANNEL_0, 0, channel_0);
setPWM(CHANNEL_1, 0, channel_1);
setPWM(CHANNEL_2, 0, channel_2);
setPWM(CHANNEL_3, 0, channel_3);
setPWM(CHANNEL_4, 0, channel_4);
setPWM(CHANNEL_5, 0, channel_5);
setPWM(CHANNEL_6, 0, channel_6);
setPWM(CHANNEL_7, 0, channel_7);
setPWM(CHANNEL_8, 0, channel_8);
setPWM(CHANNEL_9, 0, channel_9);
setPWM(CHANNEL_10, 0, channel_10);
setPWM(CHANNEL_11, 0, channel_11);
setPWM(CHANNEL_12, 0, channel_12);
setPWM(CHANNEL_13, 0, channel_13);
setPWM(CHANNEL_14, 0, channel_14);
setPWM(CHANNEL_15, 0, channel_15);
/*
led.setPWM(0, 0, 256);
led.setPWM(1, 0, 512);
led.setPWM(2, 0, 768);
led.setPWM(3, 0, 1024);
led.setPWM(4, 0, 1280);
led.setPWM(5, 0, 1536);
led.setPWM(6, 0, 1792);
led.setPWM(7, 0, 2048);
led.setPWM(8, 0, 2304);
led.setPWM(9, 0, 2560);
led.setPWM(10, 0, 2816);
led.setPWM(11, 0, 3072);
led.setPWM(12, 0, 3328);
led.setPWM(13, 0, 3584);
led.setPWM(14, 0, 3840);
led.setPWM(15, 0, 4096);
*/
}
void setPWM(const unsigned char channel, const unsigned int pulse) {
static unsigned int pulses[6];
pulses[channel]=pulse;
setPWM(1 << channel,pulses);
}
void TwigMotor::release(unsigned char motor_addr){
this->address = motor_addr;
setPWM(0);
setChannel(0b00000000);
}
//-----------------------------------------------------------------------------
// MAIN STARTS HERE -----------------------------------------------------------
int main()
{
// BEGIN
Initialise(); // Function to do initial setup
while(1) // Infinite loop
{
if (F.I2C == 1) // I2C message was received
{
// Perform some I2C operation
// If read, reset Data and PID information
// Perform operation for Target
Target = i2cTarget; // Receive velocity from CPU
setDirection(MOTOR_DIRECTION); // Set direction of the motor to the direction
// sent from CPU
OdometryCounts = 0; // Reset the variables for PID and odometry
if (i2cTarget == 0) // This to ensure a sharp stop
{
setPWM(0);
TargetZeroFlag = 1;
}
// Clear Flag
F.I2C = 0;
}
if (F.DIR == 1)
{
// Update counts before updating direction
updateOdometry();
// Update direction
DirectionRead = PORTBbits.RB5;
// Clear Flag
F.DIR = 0;
}
// PID Loop occurs at regular time intervals
if (F.T0 == 1)
{
// Update to most recent encoder counts
updateEncoder();
// If we recieve a stop command then just skip the first PID loop
// This gives the motor enough time to react to the stop command
// Without it, the motor will jitter a little bit after a stop command
// is sent.
if (TargetZeroFlag == 1)
{
AccumulatedError = 0;
EncoderCounts = 0;
TargetZeroFlag = 0;
}
/*************** Calculate stuff for PID **********************************************/
Error = Target - EncoderCounts;
AccumulatedError = AccumulatedError + Error;
DeltaError = Error - PreviousError;
PreviousError = Error;
// Set bounds for the accumulated error ///
if (AccumulatedError > 10000)
AccumulatedError = 10000;
else if (AccumulatedError < -10000)
AccumulatedError = -10000;
// Performing the actual PID
PID = (Error * KP) + (AccumulatedError * KI) + (DeltaError * KD ); // Performing PID calculation
//Note: Even through the variable types are
// different, the result should be the correct
// value of with the type of int
if (Error != 0) // If no error
{
CurrentPwm = PID + PWM_OFFSET;
if (CurrentPwm >= 255) // PWM should be between 0 - 255
CurrentPwm = 255;
else if (CurrentPwm < 0)
CurrentPwm = 0;
setPWM(CurrentPwm); // set new PWM
}
F.T0 = 0; // reset TMR0 flag
} // end PID Loop
} // end while(1)
return 1; // standard ending for an "int main"
} // END MAIN ---------------------------------------------------------------
void Motor::enableMotor() {
setPWM(0.0);
digitalWrite(MOTOR_DRIVER_ENABLE, HIGH);
motorEnabled = true;
}
int main(void) {
const static unsigned char pwmValues[]={3, 15, 40, 90};
/* ADC */
TRISBbits.TRISB14 = 1; // RB14 digital output disconnected
AD1PCFGbits.PCFG14 = 0; // RB14 configured as analog input (AN4)
AD1CON1bits.SSRC = 7; // Conversion trigger selection bits: in this // modeaninternalcounterendssamplingand
AD1CON1bits.CLRASAM = 1; // Stop conversions when the 1st A/D converter
AD1CON1bits.ON = 1;
AD1CON3bits.SAMC = 16; // Sample time is 16 TAD (TAD = 100 ns)
AD1CHSbits.CH0SA = 14;
AD1CON2bits.SMPI = 7; // uma conversao consecutiva
IPC6bits.AD1IP = 3; // valor aleatório entre 1 e 6, 0 é inativo e 7 prioridade máxima. Como só há uma interrupção pode ser qualquer valor entre 1 e 6
IEC1bits.AD1IE = 1; // autorizar interrupções pelo módulo A/D
IFS1bits.AD1IF = 0; // Quando a conversao estiver concluida, este valor deve ser 1
/* TIMER 3 */
T3CONbits.TCKPS = 2;
PR3 = 49999; // Fout = 20MHz / (32 * (39061,5 + 1)) = 2 Hz
TMR3 = 0; // Reset timer T3 count register
T3CONbits.TON = 1; // Enable timer T3 (must be the last command of the // timer configuration sequence)
// configuraçao das interrupts tabela 7.1
IPC3bits.T3IP = 4; // Configurar a prioridade relativa
IFS0bits.T3IF = 0; // Reset do Instruction Flag
IEC0bits.T3IE = 1; // Autorizar a interrupção
/* UART1 */
configUart(115200,'N',1);
OC1CONbits.OCM = 6; // ativação do modo PWM
OC1CONbits.OCTSEL = 1; // 1 ou 0 - ver página 16.8
OC1CONbits.ON = 1; // ativação do módulo
EnableInterrupts(); // Autorização globalmente o sistema de interrupções
AD1CON1bits.ASAM = 1;
while(1){
int portval = (PORTE >> 4) & 0x3;
switch (portval) {
case 0:
IEC0bits.T1IE = 1;
setPWM(0);
break;
case 1:
IEC0bits.T1IE = 1;
setPWM(100);
break;
case 2:
IEC0bits.T1IE = 0;
int dc = (PORTE >> 6) & 0x3;
setPWM(pwmValues[dc]);
break;
}
IEC0bits.T1IE = 0;
};
return 0;
}
void Motor::disableMotor() {
digitalWrite(MOTOR_DRIVER_ENABLE, LOW);
setPWM(0.0);
motorEnabled = false;
}
void setupCw1MotorPoewrLevel(unsigned short CW1) {
motorPowerLevel_CW1 = CW1;
setPWM(SOFT_PWM_CW1, motorPowerLevel_CW1);
}
Oscillator::Oscillator(void)
{
m_tempo = 120.0f;
m_sync = true;
m_waveform = k_sine;
m_startphase = 0.0;
m_phase=m_startphase;
m_pitch = 0.0f;
m_detune = 0.0f;
m_semi = 0.0f;
m_pw = 0.0f;
m_pwm = 0.0f;
m_pw_mod = 0.0f;
m_pitch_mod = 0.0f;
//noise generation
m_gaussian_noise_constant_a = 0x67452301;
m_gaussian_noise_constant_c = static_cast<int>(0xefcdab89);
m_sampleandhold_period=1;
m_sampleandhold_counter=0;
m_sample = 0.0f;
m_fintmax = static_cast<float>(std::numeric_limits<int>::max());
isActive=false;
setOscMode(k_oscmode_oscillator);
setDetune(0.5f);
setSemi(0.5f);
setVolume(1.0f);
setPW(0.5f);
setPWM(0.0f);
setPitch(440.0f);
/* populate bpm denominators
4 full notes = BPM / 960 => speed 0
Dotted 2 full notes = BPM / 720
Triplets for 4 full notes = BPM / 640
2 full notes = BPM / 480
Dotted full note = BPM / 360
Triplets for 2 full notes = BPM / 320
Full note = BPM / 240
Dotted-half note = BPM / 180
Triplet-full note = BPM / 160
Half note = BPM / 120
Dotted-quarter note = BPM / 90
Triplet-half note = BPM / 80
Quarter note = BPM / 60
Dotted-eighth note = BPM / 45
Triplet-quarter note = BPM / 40
Eighth note = BPM / 30
Dotted-sixteenth note = BPM / 22.5
Triplet-eighth note = BPM / 20
Sixteenth note = BPM / 15
Dotted-32th note = BPM / 11.25
Triplet-sixteenth note = BPM / 10
32th note = BPM / 7.5
Dotted-64th note = BPM / 5.5125
Triplet-32th note = BPM / 5
64th note = BPM / 3.75
Dotted-128th note = BPM / 2.75625
Triplet-64th note = BPM / 2.5
128th note = BPM / 1.875
Triplet-128th note = BPM / 1.25 => speed g_num_bpm_synced_lfo_speeds-1
*/
//TODO: less hardcoding
m_bpm_denominators[0]=960.0f;
m_bpm_denominators[1]=720.0f;
m_bpm_denominators[2]=640.0f;
m_bpm_denominators[3]=480.0f;
m_bpm_denominators[4]=360.0f;
m_bpm_denominators[5]=320.0f;
m_bpm_denominators[6]=240.0f;
m_bpm_denominators[7]=180.0f;
m_bpm_denominators[8]=160.0f;
m_bpm_denominators[9]=120.0f;
m_bpm_denominators[10]=90.0f;
m_bpm_denominators[11]=80.0f;
m_bpm_denominators[12]=60.0f;
m_bpm_denominators[13]=45.0f;
m_bpm_denominators[14]=40.0f;
m_bpm_denominators[15]=30.0f;
m_bpm_denominators[16]=22.5f;
m_bpm_denominators[17]=20.0f;
m_bpm_denominators[18]=15.0f;
m_bpm_denominators[19]=11.25f;
m_bpm_denominators[20]=10.0f;
m_bpm_denominators[21]=7.5f;
m_bpm_denominators[22]=5.5125f;
m_bpm_denominators[23]=5.0f;
m_bpm_denominators[24]=3.750f;
m_bpm_denominators[25]=2.75625f;
m_bpm_denominators[26]=2.5f;
m_bpm_denominators[27]=1.875f;
m_bpm_denominators[28]=1.25f;
}
// Set pwm duty in us order
void Adafruit_PWMServoDriver::setDuty(uint8_t num, uint16_t duty) {
float pulselength = 10000; // 10,000 us per second
duty = 4094 * duty / pulselength;
setPWM(num, 0, duty);
}
GPIO_Result GPIOPwmPin::startPWM(bool isTone, int durationMs) {
return setPWM(pwmFreq, pwmDuty, isTone, durationMs);
}
void main(void)
{
int i=0;
// Declare your local variables here
unsigned char devices;
devices = w1_init();
// Crystal Oscillator division factor: 1
#pragma optsize-
CLKPR=0x80;
CLKPR=0x00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif
// Input/Output Ports initialization
// Port B initialization
// Func7=Out Func6=Out Func5=Out Func4=Out Func3=Out Func2=Out Func1=Out Func0=Out
// State7=0 State6=0 State5=0 State4=0 State3=0 State2=0 State1=0 State0=0
PORTB=0x00;
DDRB=0xFF;
// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x01;
DDRC=0x01;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0b00000000;
DDRD=0b01000000;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0A output: Disconnected
// OC0B output: Disconnected
TCCR0A=0x00;
TCCR0B=0x00;
TCNT0=0x00;
OCR0A=0x00;
OCR0B=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2A output: Disconnected
// OC2B output: Disconnected
ASSR=0x00;
TCCR2A=0x00;
TCCR2B=0x00;
TCNT2=0x00;
OCR2A=0x00;
OCR2B=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// Interrupt on any change on pins PCINT0-7: Off
// Interrupt on any change on pins PCINT8-14: Off
// Interrupt on any change on pins PCINT16-23: Off
EICRA=0x00;
EIMSK=0x00;
PCICR=0x00;
// Timer/Counter 0 Interrupt(s) initialization
TIMSK0=0x00;
// Timer/Counter 1 Interrupt(s) initialization
TIMSK1=0x00;
// Timer/Counter 2 Interrupt(s) initialization
TIMSK2=0x00;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART0 Mode: Asynchronous
// USART Baud Rate: 9600
UCSR0A=0x00;
UCSR0B=0x18;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x33;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
ADCSRB=0x00;
DIDR1=0x00;
// ADC initialization
// ADC disabled
ADCSRA=0x00;
// SPI initialization
// SPI disabled
SPCR=0x00;
// TWI initialization
// TWI disabled
TWCR=0x00;
//printf("%u", devices);
TCCR0A=0b10100011; //выбираем неинверсный режим шим дл¤ обоих светодиодов
TCCR0B=0b00000001;
for(i=0;i<256;i++) //увеличиваем ¤ркость первого диода, и уменьшаем ¤ркость второго каждые 10 мс
{
OCR0A=i;
delay_ms(100);
}
while (1)
{
writetemp();
setPWM(temp);
PORTB=0x00;
delay_ms(3000);
PORTB=0x01;
delay_ms(3000);
}
/*
devices=w1_search( DS18B20_SEARCH_ROM_CMD, RomCode );
if( devices )
{
//printf("DS18B20 = ");
printf(devices);
// lcd_gotoxy( 0,1 ); lcd_puts( LcdBuffDevices ); lcd_gotoxy( 0,0 ); lcd_puts( str1 );
ds18b20_init( &RomCode[0][0], 30, 60, DS18B20_12BIT_RES );
// ds18b20_init( &RomCode[1][0], 30, 60, DS18B20_12BIT_RES );
// printf((int)DS18B20_12BIT_RES);
while (1)
{
data=UDR0;
printf("t1 %.2f \xefC", ds18b20_temperature( &RomCode[0][0] ) );
if(data=='1')
{
PORTB=0xFF;
putchar('1');
printf("Hello, world!");
}
if(data=='0')
{
PORTB=0x00;
putchar('0');
}
delay_ms(1000);
}
}
else { printf("No devices");}
*/
}
void main()
{
char buf[11];
int button;
// Initialize board to defaults
brdInit();
// Setup interrupt on channel 0 and attach button_press ISR handler
// IER mask not yet known, so set it to zero for now
bp_handle = addISRIn(0, 0, &button_press);
// Setup interrupt on channel 3 and attach rollover ISR handler
ro_handle = addISROut(3, BL_IER_ROLL_I, &rollover);
if (bp_handle < 0 || ro_handle < 0)
{
// Error: need to increase the number of interrupts allowed. (RSB_MAX_ISR)
printf("Error: need to increase number of interrupts allowed. " \
"(RSB_MAX_ISR)");
exit(-ENOSPC);
}
// Setup external interrupt on falling edge for channel 0
setExtInterrupt(0, BL_IRQ_FALL, bp_handle);
// Setup PWM on channel 3 with 5 Hz frequency
setPWM(3, 5.0, 50.0, 0, 0);
// Initialize event flags, counter and time stamp
rollover_event = 0;
button_event = 0;
button = 0;
rollover_count = 0;
time_stamp = 0;
// Enable both ISR's
enableISR(bp_handle, 1);
enableISR(ro_handle, 1);
// Write initial STDIO window strings
DispStr(1, 2, "Interrupt Service Sample");
DispStr(1, 4, "Rollover Count: 0");
// Foreground event processing loop
while (1)
{
// Look for counter rollover event
if (rollover_event)
{
// Update count on the STDIO window
sprintf(buf, "%10lu", rollover_count);
DispStr(17, 4, buf);
rollover_event = 0;
}
// Look for button press event
if (button_event)
{
// See if button message is not currently displayed
if (!button)
{
DispStr(1, 7, "Button Pressed");
button = 1;
}
button_event = 0;
}
// Look for button message timeout
if (button && ((MS_TIMER - time_stamp) > 1000))
{
// Clear message on screen and message flag
DispStr(1, 7, " ");
button = 0;
}
}
}
static void fadeLED(void) {
if (_fullLEDMode == 1) {
if (pwmDir == 0)
setPWM((uint8_t) (getBrightness(pwmCounter / PWM_SPEED)));
else if (pwmDir == 2)
setPWM((uint8_t) (getBrightness(PWM_MAX - pwmCounter / PWM_SPEED)));
else if (pwmDir == 1 || pwmDir == 3)
pwmCounter++;
// pwmDir 0~3 : idle
if (pwmCounter >= PWM_MAX * PWM_SPEED) {
pwmCounter = 0;
pwmDir = (pwmDir + 1) % 4;
}
pwmCounter++;
} else if (_fullLEDMode == 3) {
// 일정시간 유지
if (downLevelStay > 0) {
downLevelStay--;
} else {
// 시간이 흐르면 레벨을 감소 시킨다.
if (downLevelLife > 0) {
pwmCounter++;
if (pwmCounter >= PWM_SPEED) {
pwmCounter = 0;
downLevelLife--;
downLevel = downLevelMax
- (PWM_MAX - downLevelLife)
/ (PWM_MAX / downLevelMax);
}
} else {
downLevel = 0;
pwmCounter = 0;
}
}
setPWM((uint8_t) (getBrightness(downLevelLife)));
} else if (_fullLEDMode == 4) {
// 일정시간 유지
if (downLevelStay > 0) {
downLevelStay--;
} else {
// 시간이 흐르면 레벨을 증가 시킨다.
if (downLevelLife < PWM_MAX) {
pwmCounter++;
if (pwmCounter >= PWM_SPEED) {
pwmCounter = 0;
downLevelLife++;
downLevel = downLevelMax
- (PWM_MAX - downLevelLife)
/ (PWM_MAX / downLevelMax);
}
} else {
downLevel = downLevelMax;
pwmCounter = 0;
}
}
setPWM((uint8_t) (getBrightness(downLevelLife)));
} else {
pwmCounter = 0;
pwmDir = 3;
}
}
/****************************************************************************
Function:
int main(void)
Summary:
main function
Description:
main function
Precondition:
None
Parameters:
None
Return Values:
int - exit code for main function
Remarks:
None
***************************************************************************/
int main(void)
{
DWORD size = 0;
BOOL responseNeeded;
BYTE mode = 0;
BYTE wasMode = 0;
BYTE pushButtonValues = 0xFF;
BYTE potPercentage = 0xFF;
BOOL buttonsNeedUpdate = FALSE;
BOOL potNeedsUpdate = FALSE;
BOOL motorON = FALSE;
BOOL readyToRead = TRUE;
BOOL writeInProgress = FALSE;
BYTE tempValue = 0xFF;
BYTE errorCode;
ACCESSORY_APP_PACKET* command_packet = NULL;
CLKDIV = 0; /* set for default clock operations Fcyc = 4MHz */
AD1PCFGL = 0xffff;
AD1PCFGH = 0x0003;
BOOL connected_to_app = FALSE;
BOOL need_to_disconnect_from_app = FALSE;
#if defined(__PIC32MX__)
InitPIC32();
#endif
#if defined(__dsPIC33EP512MU810__) || defined (__PIC24EP512GU810__)
// Configure the device PLL to obtain 60 MIPS operation. The crystal
// frequency is 8MHz. Divide 8MHz by 2, multiply by 60 and divide by
// 2. This results in Fosc of 120MHz. The CPU clock frequency is
// Fcy = Fosc/2 = 60MHz. Wait for the Primary PLL to lock and then
// configure the auxilliary PLL to provide 48MHz needed for USB
// Operation.
PLLFBD = 38; /* M = 60 */
CLKDIVbits.PLLPOST = 0; /* N1 = 2 */
CLKDIVbits.PLLPRE = 0; /* N2 = 2 */
OSCTUN = 0;
/* Initiate Clock Switch to Primary
* Oscillator with PLL (NOSC= 0x3)*/
__builtin_write_OSCCONH(0x03);
__builtin_write_OSCCONL(0x01);
while (OSCCONbits.COSC != 0x3);
// Configuring the auxiliary PLL, since the primary
// oscillator provides the source clock to the auxiliary
// PLL, the auxiliary oscillator is disabled. Note that
// the AUX PLL is enabled. The input 8MHz clock is divided
// by 2, multiplied by 24 and then divided by 2. Wait till
// the AUX PLL locks.
ACLKCON3 = 0x24C1;
ACLKDIV3 = 0x7;
ACLKCON3bits.ENAPLL = 1;
while(ACLKCON3bits.APLLCK != 1);
TRISBbits.TRISB5 = 0;
LATBbits.LATB5 = 1;
#endif
USBInitialize(0);
AndroidAppStart(&myDeviceInfo);
responseNeeded = FALSE;
mInitPOT();
InitializeTimer2For_PWM();
PwmInit();
//InitMOTOR();
DEBUG_Init(0);
InitAllLEDs();
while(1)
{
//Keep the USB stack running
USBTasks();
//If the device isn't attached yet,
if(device_attached == FALSE || mode == 1)
{
buttonsNeedUpdate = TRUE;
potNeedsUpdate = TRUE;
need_to_disconnect_from_app = FALSE;
connected_to_app = FALSE;
size = 0;
/**/
BYTE curPush = GetPushbuttons();
if ((curPush == 0x8) || (mode == 1)) {
LED0_On();
mode = 1;
if (wasMode == 0) {
pot2LEDs();
PwmInit();
}
tempValue = ReadPOT();
wasMode = 1;
//If it is different than the last time we read the pot, then we need
// to send it to the Android device
if(tempValue != potPercentage) {
potNeedsUpdate = TRUE;
//setRPM(tempValue);
setPWM();
}
}
if ((curPush == 0x4) || (mode == 0)) {
mode = 0;
//LED0_Off();
if (wasMode == 1) {
SetLEDs(0b00000000);
wasMode = 0;
setRPM(0);
}
//Reset the accessory state variables
InitAllLEDs();
//Continue to the top of the while loop to start the check over again.
continue;
}
/* //Reset the accessory state variables
InitAllLEDs();
//Continue to the top of the while loop to start the check over again.
continue;
}*/
//}
}
//If the accessory is ready, then this is where we run all of the demo code
if(readyToRead == TRUE && mode == 0)
{
errorCode = AndroidAppRead(device_handle, (BYTE*)&read_buffer, (DWORD)sizeof(read_buffer));
//If the device is attached, then lets wait for a command from the application
if( errorCode != USB_SUCCESS)
{
//Error
DEBUG_PrintString("Error trying to start read");
}
else
{
readyToRead = FALSE;
}
}
size = 0;
if(AndroidAppIsReadComplete(device_handle, &errorCode, &size) == TRUE)
{
//We've received a command over the USB from the Android device.
if(errorCode == USB_SUCCESS)
{
//Maybe process the data here. Maybe process it somewhere else.
command_packet = (ACCESSORY_APP_PACKET*)&read_buffer[0];
}
else
{
//Error
DEBUG_PrintString("Error trying to complete read request");
}
}
while(size > 0)
{
if(connected_to_app == FALSE)
{
if(command_packet->command == COMMAND_APP_CONNECT)
{
connected_to_app = TRUE;
need_to_disconnect_from_app = FALSE;
}
}
else
{
switch(command_packet->command)
{
case COMMAND_SET_LEDS:
SetLEDs(command_packet->data);
break;
case COMMAND_APP_DISCONNECT:
need_to_disconnect_from_app = TRUE;
break;
case COMMAND_SET_PWM:
setRPM(command_packet->data);
break;
default:
//Error, unknown command
DEBUG_PrintString("Error: unknown command received");
break;
}
}
//All commands in this example are two bytes, so remove that from the queue
size -= 2;
//And move the pointer to the next packet (this works because
// all command packets are 2 bytes. If variable packet size
// then need to handle moving the pointer by the size of the
// command type that arrived.
command_packet++;
if(need_to_disconnect_from_app == TRUE)
{
break;
}
}
if(size == 0)
{
readyToRead = TRUE;
}
//Get the current pushbutton settings
tempValue = GetPushbuttons();
//If the current button settings are different than the last time
// we read the button values, then we need to send an update to the
// attached Android device
if(tempValue != pushButtonValues)
{
buttonsNeedUpdate = TRUE;
pushButtonValues = tempValue;
}
//Get the current potentiometer setting
tempValue = ReadPOT();
//If it is different than the last time we read the pot, then we need
// to send it to the Android device
if(tempValue != potPercentage)
{
potNeedsUpdate = TRUE;
potPercentage = tempValue;
}
//If there is a write already in progress, we need to check its status
if( writeInProgress == TRUE )
{
if(AndroidAppIsWriteComplete(device_handle, &errorCode, &size) == TRUE)
{
writeInProgress = FALSE;
if(need_to_disconnect_from_app == TRUE)
{
connected_to_app = FALSE;
need_to_disconnect_from_app = FALSE;
}
if(errorCode != USB_SUCCESS)
{
//Error
DEBUG_PrintString("Error trying to complete write");
}
}
}
if((need_to_disconnect_from_app == TRUE) && (writeInProgress == FALSE))
{
outgoing_packet.command = COMMAND_APP_DISCONNECT;
outgoing_packet.data = 0;
writeInProgress = TRUE;
errorCode = AndroidAppWrite(device_handle,(BYTE*)&outgoing_packet, 2);
if( errorCode != USB_SUCCESS )
{
DEBUG_PrintString("Error trying to send button update");
}
}
if(connected_to_app == FALSE)
{
//If the app hasn't told us to start sending data, let's not do anything else.
continue;
}
//If we need up update the button status on the Android device and we aren't
// already busy in a write, then we can send the new button data.
if((buttonsNeedUpdate == TRUE) && (writeInProgress == FALSE))
{
outgoing_packet.command = COMMAND_UPDATE_PUSHBUTTONS;
outgoing_packet.data = pushButtonValues;
errorCode = AndroidAppWrite(device_handle,(BYTE*)&outgoing_packet, 2);
if( errorCode != USB_SUCCESS )
{
DEBUG_PrintString("Error trying to send button update");
}
buttonsNeedUpdate = FALSE;
writeInProgress = TRUE;
}
//If we need up update the pot status on the Android device and we aren't
// already busy in a write, then we can send the new pot data.
if((potNeedsUpdate == TRUE) && (writeInProgress == FALSE))
{
outgoing_packet.command = COMMAND_UPDATE_POT;
outgoing_packet.data = potPercentage;
errorCode = AndroidAppWrite(device_handle,(BYTE*)&outgoing_packet, 2);
if( errorCode != USB_SUCCESS )
{
DEBUG_PrintString("Error trying to send pot update");
}
potNeedsUpdate = FALSE;
writeInProgress = TRUE;
}
} //while(1) main loop
}
void stopMotor(Command motor) {
setPWM(motor, 0); // Set low
}
int main(int argc, char** argv)
{
FILE* fp = NULL;
int sockfd, newsockfd, portno;
socklen_t clilen;
char buffer[256];
struct sockaddr_in serv_addr, cli_addr;
int n;
if (argc < 2) {
fprintf(stderr,"ERROR, no port provided\n");
exit(1);
}
portno = atoi(argv[1]);
if (portno <= 0)
return record(argv[2]);
if (argc > 2)
{
fp = fopen(argv[2], "wb");
if (fp == NULL)
fprintf(stderr, "\nFile for command log has not been open.\n");
}
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
error("ERROR opening socket");
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(portno);
if (bind(sockfd, (struct sockaddr *) &serv_addr,
sizeof(serv_addr)) < 0)
error("ERROR on binding");
int flag = 1;
int result = setsockopt(sockfd, /* socket affected */
IPPROTO_TCP, /* set option at TCP level */
TCP_NODELAY, /* name of option */
(char *) &flag, /* the cast is historical
craft */
sizeof(int)); /* length of option value */
if (result < 0)
error("ERROR on setting TCP_NODELAY option");
listen(sockfd,5);
clilen = sizeof(cli_addr);
newsockfd = accept(sockfd,
(struct sockaddr *) &cli_addr,
&clilen);
if (newsockfd < 0)
error("ERROR on accept");
bzero(buffer,256);
int fd = serialPort::open_port(PORT_NAME);
if (fd == -1)
return 1;
serialPort::configure_port(fd);
setPWM(fd, B_NTR, A_NTR);
struct timeval tv;
while (true)
{
n = read(newsockfd, buffer, sizeof(WORD));
if (n < 0)
{
if (fp != NULL) fclose(fp);
error("ERROR reading from socket");
}
if (buffer[0] & XINPUT_GAMEPAD_START)
{
if (fp != NULL) fclose(fp);
puts("Received START. Exiting.");
break;
}
printf("Here is the message: %04x\n", (buffer[1] << 8) | buffer[0]);
gettimeofday(&tv, NULL);
if ( buffer[0] & XINPUT_GAMEPAD_DPAD_DOWN)
{
printf("DOWN Key pressed\n");
renewB(&tv, false);
printf("Now B is %d\n", channelB);
}
if ( buffer[0] & XINPUT_GAMEPAD_DPAD_UP)
{
printf("UP Key pressed\n");
renewB(&tv, true);
printf("Now B is %d\n", channelB);
}
if ( buffer[0] & XINPUT_GAMEPAD_DPAD_LEFT)
{
printf("LEFT Key pressed\n");
renewA(&tv, false);
printf("Now A is %d\n", channelA);
}
if ( buffer[0] & XINPUT_GAMEPAD_DPAD_RIGHT)
{
printf("RIGHT Key pressed\n");
renewA(&tv, true);
printf("Now A is %d\n", channelA);
}
if ( buffer[0] & ROBOT_STRAIGHT)
{
printf("STRAIGHT is received\n");
channelA = A_NTR;
printf("Now A is %d\n", channelA);
}
if ( buffer[0] & ROBOT_CONTINUE)
{
printf("CONTINUE is received\n");
}
setPWM(fd, channelB, channelA);
if (fp != NULL)
{
fwrite(&(tv.tv_sec), sizeof(tv.tv_sec), 1, fp);
fwrite(&(tv.tv_usec), sizeof(tv.tv_usec), 1, fp);
fwrite(buffer, sizeof(WORD), 1, fp);
double distance;
getDistanceFromSonar(fd, distance);
fwrite(&distance, sizeof(double), 1, fp);
}
}
n = write(newsockfd,"I got your message", 18);
if (n < 0) error("ERROR writing to socket");
close(newsockfd);
close(sockfd);
// set defaults
setPWM(fd, B_NTR, A_NTR);
serialPort::close_port(fd);
return 0;
}
