int16_t main(void) {
InitUSB(); // initialize the USB registers and serial interface engine
initChip();
initMotor();
led_on(&led1);
timer_setPeriod(LED_TIMER, 0.5); //start internal clock with defined period
timer_start(LED_TIMER);
pin_write(IN1, 1);
pin_write(IN2, 0);
pin_write(D2, 65536*2/5);
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
while (1) {
ServiceUSB(); // service any pending USB requests
//LED1 TOGGLE TO CONFIRM RUNNING CODE
if (timer_flag(LED_TIMER)) {
timer_lower(LED_TIMER);
led_toggle(&led1);
pin_write(D2, VAL1);
}
}
}
bool DeviceELMOSteeringMotorVel::initDevice()
{
SDOManager* SDOManager = bus_->getSDOManager();
// SDOManager->addSDO(new SDONMTResetCommunication(deviceParams_->inSDOSMId_, deviceParams_->outSDOSMId_, nodeId_));
// SDOManager->addSDO(new SDONMTResetNode(deviceParams_->inSDOSMId_, deviceParams_->outSDOSMId_, nodeId_));
// printf("NMT: Enter Pre-Operational\n");
SDOManager->addSDO(new SDONMTEnterPreOperational(deviceParams_->inSDOSMId_, deviceParams_->outSDOSMId_, nodeId_));
SDOManager->addSDO(new SDOSetCOBIDSYNC(deviceParams_->inSDOSMId_, deviceParams_->outSDOSMId_, nodeId_, 0x80));
/* configure the PDOs on the motor controller */
configTxPDOs();
configRxPDOs();
/* configure several motor parameters */
setMotorParameters();
initMotor();
// printf("NMT: Start remote node\n");
SDOManager->addSDO(new SDONMTStartRemoteNode(deviceParams_->inSDOSMId_, deviceParams_->outSDOSMId_, nodeId_));
return true;
}
void motor(){
if(motorStateChange!=motorState){
motorStateChange = motorState;
{
int8_t difference = currentPosition - lastPosition;
if(difference > 0 && motorState == 4)
{
currentPosition--;
}
else if (difference < 0 && motorState == 3)
{
currentPosition++;
}
}
}
if(TCNT1>SECOND/speedModifier){
TCNT1 = 0;
switch(motorState){
case 0:
stopMotor();
return;
case 1:
// motor idle...
if(!motorInitialized){
initMotor();
motorInitialized = 1;
}
return;
case 3:
/* double forward motion */
if(steps>0){
steps--;
lastPosition = currentPosition-1;
currentPosition++;
moveMotor();
} else {
motorState = 1;
}
return;
case 4:
/* double backward motion */
if(steps>0){
steps--;
lastPosition = currentPosition+1;
currentPosition--;
moveMotor();
} else {
motorState = 1;
}
return;
}
}
}
/* User Control */
task usercontrol()
{
initMotor(&motorLeftShooter, leftShooter1, leftShooter2, leftShooterSensor);
initMotor(&motorRightShooter, rightShooter1, rightShooter2, rightShooterSensor);
clearAll(actOnSensors);
clearTimer(T1);
clearTimer(T2);
while(true)
{
drive();
intake();
shooter(&motorLeftShooter, &motorRightShooter);
velocidade_motor_esquerdo=motorLeftShooter.speedRead;
potencia_motor_esquerdo=motorLeftShooter.controller_output;
velocidade_motor_direito=motorRightShooter.speedRead;
potencia_motor_direito=motorRightShooter.controller_output;
}
}
int main()
{
initGPIO();
initMotor();
/* Setup MYTIMER */
MYTIMER_init();
//MYTIMER_setOverflowVal(T1_FREQ);
//MYTIMER_setCompareVal(10000); // 75% duty cycle
MYTIMER_enable_overflowInt();
//MYTIMER_enable_compareInt();
//MYTIMER_enable_allInterrupts();
NVIC_EnableIRQ(Fabric_IRQn);
MYTIMER_enable();
int j = 0, k=0,l=0;
//Fastest seen: 100 accel, 23 speed, 2000000 Tclk
move(10000,100, 1,23);
volatile int y;
for(j=0;j <1000; ++j) {
for(k=0; k<10000; ++k) {
y = 0;
}
}
move(-1000, 1,1,24);
//count = 1e6;
//while(1){
// if(!count){ // Global variable checking
// count decremented inside the interrupt service routine
// MYTIMER_disable(); // Disable after 1e6 interrupts
// }
//}
/*
while(1) {
uint32_t i;
printf("Time: %lu\r\n", MYTIMER_getCounterVal()); // Standard printf() now work
for(i=1e6; i>0; i--); // busy wait
}*/
while(1);
return 0;
}
/* Autonomous */
task autonomous(){
bool blueSide = true;
initMotor(&motorLeftShooter, leftShooter1, leftShooter2, leftShooterSensor);
initMotor(&motorRightShooter, rightShooter1, rightShooter2, rightShooterSensor);
clearAll(actOnSensors);
startTask(autonShooterControl);
if(blueSide)
{
autonTargetSpeed = 75;
wait1Msec(1200);
for(int i = 0; i < 5; i++)
{
wait1Msec(2000);
setIntake(120);
wait1Msec(500);
setIntake(0);
}
}
while(true){}
}
int main(void) {
initIRSensor();
initMotor();
initGPIOLineSensor();
initServo();
SetServo(servo,0);
stage = 0;
//while(1){followLine();}
//while(1){followWall();}
//SetMotor(leftMotor, 1); SetMotor(rightMotor, 1);
while(true){
LineSensorReadArray(gls, line);
if (stage==0){ //start state
if(line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5) {
followWall();
}else{
followLine();
}
}else if(stage==1){
//once 90degree turn passed
// SetMotor(leftMotor, 1);
// SetMotor(rightMotor, -1);
SetPin(PIN_F2, 1);
followWall();
if (wallPresent()) {followWall();}
else {
findLine();
}
SetPin(PIN_F2, 0);
}else if (stage==2){
//once line found again after walled section
SetPin(PIN_F1, 1);
if((line[0]<0.5&&line[1]<0.5&&line[2]<0.5&&line[3]<0.5&&line[4]<0.5&&line[5]<0.5&&line[6]<0.5&&line[7]<0.5)||
(mostDark())) { //line[0]>0.5&&line[1]>0.5&&line[2]>0.5&&line[3]>0.5&&line[4]>0.5&&line[5]>0.5&&line[6]>0.5&&line[7]>0.5)
findEnd();
}else{
followLine();
};
SetPin(PIN_F1, 0);
}else{//end of course look for flag
findObject();
break;
}
}
}
int main() {
STATUS ret;
char msg[256];
ret = initLcd();
ASSERT(ret == STATUS_OK);
//ret = initAdc();
//ASSERT(ret == STATUS_OK);
ret = initMotor();
ASSERT(ret == STATUS_OK);
ret = initZigbee();
ASSERT(ret == STATUS_OK);
ret = initFileSystem();
ASSERT(ret == STATUS_OK);
while (!feof(MAP_FILE)){
fscanf(MAP_FILE, "%s", msg);
printf("%s", msg);
}
while(1);
return 0;
}
int main() {
uchar i;
setHigh(DDRB, PORTB0); // make PB0 output port
setHigh(PORTB, PORTB0); // indicate that the micro-controller is active
// motor frequency timer
TCCR1B |= 1 << CS12;
// motor output pins
setHigh(DDRD,PORTD4);
setHigh(DDRD,PORTD5);
setHigh(DDRD,PORTD6);
setHigh(DDRD,PORTD7);
wdt_enable(WDTO_1S); // enable 1s watchdog timer
usbInit();
usbDeviceDisconnect(); // enforce re-enumeration
for(i = 0; i<250; i++) { // wait 500 ms
wdt_reset(); // keep the watchdog happy
_delay_ms(2);
}
usbDeviceConnect();
sei(); // Enable interrupts after re-enumeration
initMotor();
while(1) {
wdt_reset(); // keep the watchdog happy
usbPoll();
/*motor functions*/
motor();
}
return 0;
}
//int main()
void initStepper(void)
{
initGPIO(); //initialize GPIO ports 0-3
initMotor(); //initialize stepper motors
MYTIMER_init(); //initialize mytimer (hardware timer)
//MYTIMER_setOverflowVal(T1_FREQ);
//MYTIMER_setCompareVal(10000); // 75% duty cycle
MYTIMER_enable_overflowInt(); //enable overflow interrupts
//MYTIMER_enable_compareInt();
//MYTIMER_enable_allInterrupts();
NVIC_EnableIRQ(Fabric_IRQn); //enable fabric interrupt
MYTIMER_enable(); //start hardware timer
//Fastest seen: 100 accel, 23 speed, 2000000 Tclk
//move(step,accel,decel,speed); //move motor based on function call
return;
}
int main() {
MotorDirection dirs[] = {FORWARD, BACKWARD, RIGHT, LEFT,
SOFT_RIGHT, SOFT_LEFT, SOFT_RIGHT2, SOFT_LEFT2, STOP};
char strDirs[9][16] = {"FORWARD", "BACKWARD", "RIGHT", "LEFT",
"SOFT_RIGHT", "SOFT_LEFT", "SOFT_RIGHT2", "SOFT_LEFT2", "STOP"};
int idx;
initMotor();
initLcd();
#if 0
/* Test #1: Check direction */
motorVelocitySet(255, 255);
for(idx = 0; idx < (sizeof(dirs)/sizeof(dirs[0])); idx ++) {
lcdClear();
lcdCursor(1,1);
lcdString(strDirs[idx]);
_delay_ms(DELAY_COUNT*2);
motorDirectionSet(dirs[idx]);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
}
/* Test #2: Check speed */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 100, R 100");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(100, 100);
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #3: Check individual wheel speed (L < R) */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 100, R 255");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(100, 255); /* Left turn expected */
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #4: Check individual wheel speed (L > R) */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 255, R 100");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(255, 100); /* Right turn expected */
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #5: Check left position encoder */
motorLeftPositionEncoderInit(leftPosEncoderIsr);
lPosCount = 100;
motorVelocitySet(255, 255);
motorDirectionSet(FORWARD);
#endif
/* Test #6: Check orientation */
motorLeftPositionEncoderInit(leftPosEncoderIsr);
lPosCount = 23;
motorVelocitySet(150, 150);
motorDirectionSet(LEFT);
while(1);
return 0;
}
int main(void){
//SYSTEMConfigPerformance(10000000);
int i;
state = forward;
float Volts = 0.0;
float VoltNew = 0.0;
init_timer_1();
enableInterrupts();
init_lcd();
clear_lcd();
initPWM();
char voltage[8];
initADC();
initMotor();
while(1){
while(IFS0bits.AD1IF == 0) {
}
val = ADC1BUF0;
IFS0bits.AD1IF = 0;
VoltNew = (float)val*5.0/(1023.0);
Volts = VoltNew;
sprintf(voltage, "%.2f",Volts);
move_cursor_lcd(0,1);
print_string_lcd(voltage);
switch(state) {
case forward:
idle1 = 0;
OC1RS = 0;
OC2RS = 0;
if (Volts > 2.5) {;
OC3RS = 1023;
OC4RS = (1- (Volts - 2.5)/2.5)*1023;
}
else if (Volts < 2.5) {
OC3RS = (1- (2.5 - Volts)/2.5)*1023;
OC4RS = 1023;
}
else {
OC3RS = 1023;
OC4RS = 1023;
}
stateNext = back;
break;
case back:
idle1 = 0;
OC3RS = 0;
OC4RS = 0;
if (Volts > 2.5) {;
OC1RS = 1023;
OC2RS = (1- (Volts - 2.5)/2.5)*1023;
}
else if (Volts < 2.5) {
OC1RS = (1- (2.5 - Volts)/2.5)*1023;
OC2RS = 1023;
}
else {
OC1RS = 1023;
OC2RS = 1023;
}
stateNext = forward;
break;
case idle:
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = 0;
idle1 = 1;
break;
case debounce1:
delay_ms(40);
break;
case debounce2:
delay_ms(40);
if(idle1 == 1)
state = stateNext;
else if (idle1 == 0)
state = idle;
break;
}
}
return 0;
}
int main(void){
//SYSTEMConfigPerformance(10000000);
int i = 0;
int j = 0;
int onLine = 0;
float Volts = 0.0;
init_timer_1();
enableInterrupts();
init_lcd();
clear_lcd();
initPWM();
char voltage[8];
initSW1();
initADC();
initMotor();
move_cursor_lcd(0,1);
print_string_lcd("lLLLLL");
PrintValue();
lineL = valL + 50;//780;
lineR = valR + 50;//805;
lineM = valM - 60;//840;
while(1){
switch(state) {
case Forward:
OC1RS = 0;
OC2RS = 0;
OC3RS = 200;
OC4RS = 200;
onLine = 0;
PrintValue();
if(valR > lineR && valL > lineL && valM > lineM) {
state = Double;
}
else if (valL > lineL) {
onLine = 0;
state = TurnLeft;
if (valM > lineM + 50/*&& counter == 0*/) state = Forward;
}
else if (valR > lineR) {
onLine = 0;
state = TurnRight;
if (valM > lineM /*&& counter == 0*/) state = Forward;
//else state = TurnRight;
}
else onLine = 0;
sprintf(stateDisp, "For");
break;
//All sensors are over a line
case Double:
OC1RS = 0;
OC2RS = 0;
OC3RS = 200;
OC4RS = 200;
if (onLine == 0) {
counter += 1;
onLine = 1;
}
sprintf(stateDisp,"Doub");
PrintValue();
// delay_ms(100);
//if(!(valR > (lineR + Thresh) && valL > (lineL + Thresh) && valM > (lineM + Thresh))) state = Forward;
if (counter >= 1) state = TurnAround;
//if (counter > 5) state = TurnLeft;
if (counter == 12) state = Idle;
break;
case TurnAround:
sprintf(stateDisp,"Tur");
OC1RS = 100;
OC2RS = 0;
OC3RS = 100;
OC4RS = 0;
onLine = 0;
PrintValue();
stateNext = TurnAround;
delay_ms(1);
if (valR > lineR /*&& valR < lineR*/ && i == 1) {
j = 1;
}
if(valR < lineR) i = 1;
if (j == 1 && valM > lineM + Thresh) {
stateNext = Forward;
i = 0;
j = 0;
}
//else if(valR < lineR) state = Forward;
state = stateNext;
break;
case TurnLeft:
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = 300;
onLine = 0;
sprintf(stateDisp, "Lef");
PrintValue();
if(valL < lineL) state = Forward;
//else if (valR > lineR && valM > lineM) state = Double;
break;
case TurnRight:
OC1RS = 0;
OC2RS = 0;
OC3RS = 300;
OC4RS = 0;
onLine = 0;
sprintf(stateDisp, "Rig");
PrintValue();
if(valR < lineR) state = Forward;
//if(valL > lineL) state = TurnLeft
if(valL > lineL) state = TurnLeft;
if (valR > (lineR + Thresh) && valL > (lineL + Thresh) && valM > (lineM + Thresh)) state = Double;
//else if(counter < 1) state = Forward;
//else state = TurnRight;
break;
case Idle:
idle1 = 1;
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = 0;
sprintf(stateDisp, "Idl");
PrintValue();
break;
}
}
return 0;
}
int main(void) {
initTMR();
initSensor();
initMotor();
TRISButton = OUTPUT;
WEAKASSPULLUP = ENABLED;
TRISLEDR = OUTPUT;
TRISLEDF = OUTPUT;
TRISLEDL = OUTPUT;
while (1) {
switch (state) {
case idle:
stop();
if (button == PRESSED) state = debouncePress;
nextState = follow;
break;
case wait:
if (button == UNPRESSED) state = debounceRelease;
break;
case debouncePress:
delayUs(5000);
state = wait;
break;
case debounceRelease:
delayUs(5000);
state = nextState;
break;
case follow:
nextState = idle;
//delayUs(5000);
if (IFS0bits.AD1IF == 1) {
IFS0bits.AD1IF = 0;
front = getSensorFront();
left = getSensorLeft();
right = getSensorRight();
}
if ((right == ONTAPE || left == ONTAPE)) {
stop();
if (left == ONTAPE && front == OFFTAPE) alignLeft();
else if (right == ONTAPE && front == OFFTAPE) alignRight();
else if(right == ONTAPE && front == ONTAPE && left == ONTAPE) flip();
else forward();
}
else if (front == ONTAPE) {
forward();
}
/*else if(front == ONTAPE && (right == ONTAPE || left == ONTAPE)) {
stop();
if(turns < 3) turnRight();
else if (turns == 3) flip();
else turnLeft();
turns++;
}*/
if (button == PRESSED) state = debouncePress;
break;
}
}
}
int main(void){
int j = 0;
int onLine = 0;
init_timer_1();
enableInterrupts();
init_lcd();
clear_lcd();
initPWM();
initADC();
initMotor();
move_cursor_lcd(0,1);
print_string_lcd("XXXX"); //Though the reason is unclear, the display does not function properly without some initial string being printed on it
PrintValue();
lineL = valL + lThresh;
lineR = valR + rThresh;
lineM = valM - mThresh;
while(1){
switch(state) {
//Default state. Proceed forward, while ready to change into any other state
case Forward:
OC1RS = 0;
OC2RS = 0;
OC3RS = fSpeed;
OC4RS = fSpeed;
onLine = 0;
PrintValue();
if(valR > lineR + Thresh && valL > lineL + Thresh && valM > lineM + Thresh) {
stateNext = Triple;
state = Delay;
}
else {
if (valL > lineL + Thresh && valM > lineM + Thresh) {
Full = 1;
i = 0;
stateNext = SharpLeft;
state = Delay;
}
else if (valR > lineR + Thresh && valM > lineM + Thresh) {
Full = 1;
i = 0;
stateNext = SharpRight;
state = Delay;
}
else if (valR > lineR) {
state = TurnRight;
}
else if (valL > lineL) {
state = TurnLeft;
}
}
sprintf(stateDisp, "For");
break;
//All sensors are over a line
case Triple:
OC1RS = 0;
OC2RS = 0;
OC3RS = fSpeed;
OC4RS = fSpeed;
if (onLine == 0) {
counter += 1;
onLine = 1;
}
sprintf(stateDisp,"Trip");
PrintValue();
if (!(valR > lineR + Thresh && valL > lineL + Thresh && valM > lineM + Thresh)) {
if (counter < 3 && counter > 1) {
i = 0;
Full = 1;
stateNext = SharpRight;
state = Delay;
}
else if (counter == 3) state = TurnAround;
else if (counter > 3) state = Idle;
}
break;
//Make 180 degree turn
case TurnAround:
sprintf(stateDisp,"Tur");
OC1RS = tSpeed;
OC2RS = 0;
OC3RS = tSpeed;
OC4RS = 0;
onLine = 0;
PrintValue();
delay_ms(1);
if (valR > lineR /*&& valR < lineR*/ && i == 1) {
j = 1;
}
if(valR < lineR) i = 1;
if (j == 1 && valM > lineM + Thresh) {
i = 0;
j = 0;
state = Forward;
}
break;
//Turn left until the left sensor no longer detects the line
case TurnLeft:
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = tSpeed;
onLine = 0;
sprintf(stateDisp, "Lef");
PrintValue();
if(valL < lineL) state = Forward;
else if (valR > (lineR + Thresh) && valL > (lineL + Thresh) && valM > (lineM + Thresh)) {
state = Triple;
}
else if (valL > lineL + Thresh && valM > lineM + Thresh) {
i = 0;
Full = 1;
stateNext = SharpLeft;
state = Delay;
}
break;
//Turn right until the right sensor no longer detects the line
case TurnRight:
OC1RS = 0;
OC2RS = 0;
OC3RS = tSpeed;
OC4RS = 0;
onLine = 0;
sprintf(stateDisp, "Rig");
PrintValue();
if(valR < lineR) state = Forward;
else if (valR > (lineR + Thresh) && valL > (lineL + Thresh) && valM > (lineM + Thresh)) {
state = Triple;
}
else if (valR > lineR + Thresh && valM > lineM + Thresh) {
i = 0;
Full = 1;
stateNext = SharpRight;
state = Delay;
}
break;
//Turn right until both the right and middle sensors have passed over a new line
case SharpRight:
OC1RS = 0;
OC2RS = 0;
OC3RS = tSpeed / 2;
OC4RS = 0;
onLine = 0;
sprintf(stateDisp, "sRi");
PrintValue();
if (valM > lineM) state = Forward;
if (valR > lineR && i == 1) {
j = 1;
}
if(valR < lineR) i = 1;
if (j == 1 && valM > lineM + Thresh) {
i = 0;
j = 0;
state = Forward;
}
break;
//Turn left until both the left and middle sensors have passed over a new line
case SharpLeft:
OC1RS = 0;
OC2RS = 0;
OC3RS = 0;
OC4RS = tSpeed;
onLine = 0;
sprintf(stateDisp, "sLe");
PrintValue();
if (valM > lineM) state = Forward;
if (valL > lineL && i == 1) {
j = 1;
}
if(valR < lineR) i = 1;
if (j == 1 && valM > lineM + Thresh) {
i = 0;
j = 0;
state = Forward;
}
break;
//Halt robot. Primarily for debugging
case Idle:
idle1 = 1;
OC1RS = 0;
OC2RS = 0;
OC3RS = fSpeed;
OC4RS = fSpeed;
sprintf(stateDisp, "Idl");
PrintValue();
break;
//Continue forward until the robot has passed a 90 degree turn or perpendicular line, then take the appropriate action
case Delay:
OC1RS = 0;
OC2RS = 0;
OC3RS = fSpeed;
OC4RS = fSpeed;
PrintValue();
sprintf(stateDisp, "Del");
if (!(valR > lineR + Thresh || valL > lineL + Thresh)) state = stateNext;
break;
}
}
return 0;
}
