int main(void)
{
int i = 0;
for(; i<8; i++) {
sensors[i] = 0;
}
//Needs to be before sei.. don't ask why..
//Also needs a delay from start up..
_delay_ms(1000);
InitLCD();
sei();
SETUP_SENSOR_IO();
CLEAR_BIT(DDRE, PE5);//Input pin
ENABLE_EXTERNAL_INTERRUPT(INT5);//External Interrupt Request 5 INT5 enabled*/
InitServo(0);
InitMotor();
SetupTachometer();
float currentDirection = 0;
float targetDirection = 0;
bool terminated = false;
currentSpeed = 10;
tachoPulsesPerSecond = 0;
for (;;)
{
if(updateScreen) {
updateScreen = false;
ClearScreen();
WriteText("State:",1);
WriteText_StartingFrom(status,2,7);
WriteText("Lap:",3);
WriteText_StartingFrom(lap,4,7);
WriteText("Ticker:",5);
WriteText_StartingFrom(time,6,7);
WriteText("cycle:",7);
WriteText_StartingFrom(cyc,8,7);
}
//Terminated when lapCounter > 3
if(!terminated) {
float direction = GetFilteredSensorValue();
if (lapCounter > 3) {
SetMotorSpeed(0);
MoveServo(0);
terminated = false;
status = "Terminated";
updateScreen = true;
driveFlag = false;
lapCounter = 0;
continue;
}
targetDirection = direction * 45.0;
if (absFloat(targetDirection) >= absFloat(currentDirection)) {
currentDirection = targetDirection;
}
else {
currentDirection += (targetDirection - currentDirection);
}
MoveServo(currentDirection);
if (driveFlag) {
//Calculate new motor speed and accelerate/deccelerate
float desiredPulsePerSecond = 9 + (1.0 - absFloat(direction)) * 5;
float difference = desiredPulsePerSecond - tachoPulsesPerSecond;
float coefficient = 0.015;
if (difference < 0) {
coefficient = -0.015;
}
currentSpeed += 20 * coefficient;
if(currentSpeed>80) {
currentSpeed = 80;
}
if(currentSpeed<0) {
currentSpeed = 0;
}
SetMotorSpeed(currentSpeed);
}
}
}
return 0;
}
/* ----------------------------------------------------- */
void TestServo(void)
{
WriteLine("Initializing the servos...");
InitServos();
WriteLine("Powering on the servos...");
Power_On_Servos();
WriteLine("Loading the start positions...");
LoadStartPosition();
_delay_ms(2000);
MoveServo(1, 700);
_delay_ms(2000);
MoveServo(1, 1500);
_delay_ms(2000);
MoveServo(2, 700);
_delay_ms(2000);
MoveServo(2, 1500);
_delay_ms(2000);
MoveServo(3, 700);
_delay_ms(2000);
MoveServo(3, 1500);
_delay_ms(2000);
MoveServo(4, 700);
_delay_ms(2000);
MoveServo(4, 1500);
_delay_ms(2000);
MoveServo(5, 700);
_delay_ms(2000);
MoveServo(5, 1500);
_delay_ms(2000);
MoveServo(6, 700);
_delay_ms(2000);
MoveServo(6, 1500);
_delay_ms(2000);
Power_Off_Servos();
}
//****************************************************************************
// MAIN FUNCTION
//****************************************************************************
void main()
{
long lRetVal = 0;
char data[BUF_SIZE];
char sent_data[BUF_SIZE];
unsigned char highByte, lowByte;
int i;
unsigned long tempStartTimeStamp, tempStopTimeStamp;
memset(sent_data, 0, 10);
// Board Initialization
BoardInit();
// Configure the pinmux settings for the peripherals exercised
PinMuxConfig();
// Initialize the PWM outputs on the board
InitServos();
// Initialize the sensor ADC
InitSensorADC();
// Configuring UART
InitTerm();
// Display banner
DisplayBanner(APPLICATION_NAME);
// Connect to WIFI using default info
//WlanConnect(NULL, NULL, NULL);
WlanConnect("Nagui's Network", "SL_SEC_TYPE_WPA", "19520605");
// Setup the TCP Server Socket
BsdTcpServerSetup(PORT_NUM);
// Recieve Data
while (lRetVal >= 0)
{
lRetVal = BsdTcpServerReceive(data);
//SysTickPeriodSet(800000000); // test
//SysTickEnable(); // test
Timer_IF_Init(PRCM_TIMERA0, TIMERA0_BASE, TIMER_CFG_PERIODIC, TIMER_A, 0);
Timer_IF_Start(TIMERA0_BASE, TIMER_A, MILLISECONDS_TO_TICKS(2000));
//tempStartTimeStamp = SysTickValueGet(); // test
tempStartTimeStamp = Timer_IF_GetCount(TIMERA0_BASE, TIMER_A);
for (i = 0; i<NUM_SERVOS; i++)
{
MoveServo((unsigned char)data[i], (enum Servo_Joint_Type)i);
UART_PRINT("%d", (unsigned int) data[i]);
}
for (i = 0; i< NUM_SENSORS; i++)
{
//UnsignedShort_to_UnsignedChar(GetSensorReading((enum Fingertip_Sensor_Type)i), &highByte, &lowByte);
UnsignedShort_to_UnsignedChar(GetSensorReading(SENSOR_FINGER_INDEX), &highByte, &lowByte);
sent_data[i*2] = (char)highByte;
sent_data[i*2+1] = (char)lowByte;
}
//tempStopTimeStamp = SysTickValueGet(); // test
tempStopTimeStamp = Timer_IF_GetCount(TIMERA0_BASE, TIMER_A);
lRetVal = BsdTcpServerSend(sent_data, 10);
UART_PRINT("timestamp start: %lu\n\r", tempStartTimeStamp); //test
UART_PRINT("timestamp stop: %lu\n\r", tempStopTimeStamp); //test
UART_PRINT("Sent 10 bytes to client.\n\r");
}
UART_PRINT("Exiting Application ...\n\r");
// power of the Network processor
lRetVal = sl_Stop(SL_STOP_TIMEOUT);
}
void FindServoZeroPoints()
{
// not clean but...
int data;
short sSN; // which servo number
boolean fNew = true; // is this a new servo to work with?
boolean fExit = false; // when to exit
int ich;
word wCenter;
// OK lets move all of the servos to their zero point.
InitializeServos();
Serial.println("Find Servo Zeros.\n$-Exit, +- changes, *-change servo");
// Lets move all of the servos to their default location...
for (sSN=0; sSN < MNUMSERVOS; sSN++)
g_aServos[sSN]->SetDutyUS(rcd.aServos[sSN].wCenter);
sSN = 0; // start at our first servo.
while(!fExit) {
if (fNew) {
wCenter = rcd.aServos[sSN].wCenter;
Serial.print("Servo: ");
Serial.print(g_apszServos[sSN]);
Serial.print("(");
Serial.print(wCenter-1500, DEC);
Serial.println(")");
// Now lets wiggle the servo
MoveServo(g_aServos[sSN], wCenter+250, 500);
MoveServo(g_aServos[sSN], wCenter-250, 500);
MoveServo(g_aServos[sSN], wCenter, 500);
fNew = false;
}
//get user entered data
data = Serial.read();
//if data received
if (data !=-1) {
if (data == '$')
fExit = true; // not sure how the keypad will map so give NL, CR, LF... all implies exit
else if ((data == '+') || (data == '-')) {
if (data == '+')
rcd.aServos[sSN].wCenter += 5; // increment by 5us
else
rcd.aServos[sSN].wCenter -= 5; // increment by 5us
Serial.print(" ");
Serial.println(rcd.aServos[sSN].wCenter, DEC);
// Lets try to use attach to change the offsets...
MoveServo(g_aServos[sSN], rcd.aServos[sSN].wCenter, 100);
} else if ((data >= '0') && (data < ('0'+ MNUMSERVOS))) {
// direct enter of which servo to change
fNew = true;
sSN = data - '0';
} else if (data == '*') {
// direct enter of which servo to change
fNew = true;
sSN++;
if (sSN == MNUMSERVOS)
sSN = 0;
}
}
}
Serial.print("Find Servo exit ");
for (sSN=0; sSN < MNUMSERVOS; sSN++){
Serial.print(" ");
Serial.print(g_aServos[sSN]->GetDutyNS()/1000, DEC);
}
Serial.print("\nSave Changes? Y/N: ");
//get user entered data
while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15)))
;
if ((data == 'Y') || (data == 'y')) {
// call off to save away the updated data.
rcd.Save();
} else {
g_fServosInit = false; // Lets go back and reinit...
rcd.Load();
}
}
