void trackLineInit()
{
// Assume we start on the center of the line
lineSlope = 0;
lineOffset = 0;
topLost = TRUE;
bottomLost = TRUE;
statsBufProcessed = TRUE;
// Set pan, tilt, boom postions
setServo(BOOM, BOOM_UP);
setServo(PAN, PAN_CENTER);
setServo(TILT, TILT_FORWARD);
myDelay(5);
// Initialize wheel speeds to some base speed
setLeftPWM(LEFT_BASE_SPEED);
setRightPWM(RIGHT_BASE_SPEED);
// Set Low Resolution
rprintf("hr 0\r");
// Downsample the image
rprintf("DS %d %d\r", DS_X_LINE, DS_Y_LINE);
// Line mode type 0, mode 2
// "per row statistics in the tracked region" p.41
rprintf("lm 0 2\r");
// Output mask hides everything but, mean p.45
rprintf("om 5 15\r");
// set black line tracking parameters p.51
rprintf("st 16 70 16 70 16 60\r");
// set virtual window
rprintf("vw %d %d %d %d\r", VW_X1_LINE, VW_Y1_LINE, VW_X2_LINE, VW_Y2_LINE);
// start the tracking (Track Color)
rprintf("tc\r");
}
static void setPosSHR(UArm *uarm,PosSHR shr)
{
double s2h2,angleA,angleB,angleC;
int angleR,angleL;
uarm->status.pos = shr;
shr.r = -shr.r;
if(ifSwitchOn(uarm) && shr.h < uarm->attr.heightLst)
shr.h = uarm->attr.heightLst;
formatSHR(&shr);
s2h2 = shr.s * shr.s + shr.h * shr.h;
angleA = acos((ARM_A2B2 - s2h2) / ARM_2AB) * RAD_TO_DEG;
angleB = atan(shr.h/shr.s) * RAD_TO_DEG; //
angleC = acos((ARM_A2 + s2h2 -ARM_B2)/(2 * ARM_A * sqrt(s2h2))) * RAD_TO_DEG; //
angleR = 180 - angleA - angleB - angleC + FIXED_OFFSET_R + uarm->attr.offsetR; //
angleL = angleB + angleC + FIXED_OFFSET_L + uarm->attr.offsetL; //
// angle limit
angleL = constrain(angleL, 10 + uarm->attr.offsetL, 145 + uarm->attr.offsetL);
angleR = constrain(angleR, 25 + uarm->attr.offsetR, 150 + uarm->attr.offsetR);
angleR = constrain(angleR, angleL - 90 + uarm->attr.offsetR, angleR); // behind -120+30 = -90
if(angleL < 15 + uarm->attr.offsetL)
angleR = constrain(angleR, 70 + uarm->attr.offsetR, angleR);
setServo(uarm,SERVO_R, angleR);
setServo(uarm,SERVO_L, angleL);
setServo(uarm,SERVO_ROT, shr.r);
setServo(uarm,SERVO_HANDROT, shr.handrot);
uarm->attr.heightLst = shr.h;
}
/**********MAIN ROUTINE*************/
int main(int argc, char** argv)
{
config_init();
MOTOR_init();
lcdInit();
configSense();
//MOTOR_testMotors();
//lcdClean();
while (1) {
//encoder_move_degree(90,255,1);
//encoder_move_degree(90,255,2);
//encoder_move_degree(90,255,3);
//encoder_move_degree(90,255,4);
for( int j = 0; j < 180; j+=10)
{
setServo(j);
showTempLight();
for(int i = 0; i < 20; i++) __delay_ms(10);
}
for( int j = 180; j > 0; j-=10)
{
setServo(j);
showAccelerometerVal();
for(int i = 0; i < 20; i++) __delay_ms(10);
}
}
return (EXIT_SUCCESS);
}
int main(void)
{
initServo();
initSerial(9600);
uint16_t dest;
SerMsg msg;
char err;
_delay_ms(10); // wait a little while for things to come online.
sei();
DDRB|=(1<<PB1);
DDRB|=(1<<PB2);
DDRB|=(1<<PB5);
handshake();
setServo(PITCHPOS,512);
setServo(YAWPOS,450);
while(1)
{
err = getMessage(&msg,&dest);
if (err==0)
{
setServo(msg,dest);
PORTB|=(1<<PB5);
}
_delay_ms(20);
PORTB&=~(1<<PB5);
_delay_ms(20);
}
}
task main()
{ //Program begins, insert code within curly braces
setServo(servoMotor, -127); //Set servo to -127
wait(2); //Wait 2 sec
setServo(servoMotor, -63);
wait(3);
setServo(servoMotor, 0);
wait(2);
setServo(servoMotor, 63);
wait(3);
setServo(servoMotor, 127);
wait(2); //Wait at position 127 for 2 sec before returning by default to 0.
}
HandTask::HandTask(double gain):
_gain(gain)
{
setServo(vpServo::EYEINHAND_L_cVe_eJe);
setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
setLambda(_gain);
}
void Pololu::Usc::Usb::SetSpeed::read(std::istream& stream) {
Serializable<unsigned char> servo;
Serializable<unsigned short> value;
setServo(servo.read(stream));
setValue(value.read(stream));
}
void servoInit()
{
// set servos to initial positions
setServo(PAN, PAN_CENTER);
setServo(BOOM, BOOM_UP);
setServo(TILT, TILT_FORWARD);
setServo(DOOR, DOOR_CLOSED);
myDelay(30);
setServo(LIFT, LIFT_UP);
myDelay(50);
disableServo(PAN);
disableServo(BOOM);
disableServo(TILT);
disableServo(DOOR);
disableServo(LIFT);
}
bool steering::steer(LineSensor lineSensor, Servo motor, int speed) {
int treebeard = getAngle(lineSensor, motor.read(), speed);
if (treebeard == 314) {
return false;
} else if(treebeard == -314) {
setServo(STEERING_CENTER, motor);
return true;
}
moveServo(treebeard, motor);
//setServo(STEERING_CENTER - treebeard, motor);
/*string cheese
double dutch
char lizard
float rootbeer
long way_to_go
byte me!
short people
name cecil
enter the void
;oscapy*/
return true;
}
/**
* device_write() - Called when a process writes to the device file
*/
static ssize_t device_write(struct file *filp,
const char *buff,
size_t len,
loff_t * off)
{
int size, percent;
if (len > 4) {
size = 4;
} else {
size = len;
}
// Copy data from user space to kernel space.
if (copy_from_user(msg, buff, size)) {
return -EFAULT;
}
msg[len] = '\0';
// Convert to integer
if ( kstrtoint(msg, 10, &percent) < 0 ) {
return -EFAULT;
}
setServo(percent);
return size;
}
void navigate()
{
extern enum driveState tess_drive_state;
tess_ping = ping_cm(ULTRASONIC_PIN);
debug("Distance = %i\n", tess_ping);
if (tess_ping == 0 && tess_drive_state != IDLE)
{
tess_drive_state = IDLE;
debug("Ultrasonic Error!\n", 0);
setServo(0,0,"Error: in void navigate()\n");
}
else
{
if (tess_ping < OBJECT_DISTANCE && tess_drive_state != TURNING)
{
//TODO stop and turn
stop();
debug("Turning on LED!\n", 0);
high(LED_1);
debug("About to turn!\n", 0);
turn(LEFT); //enum from drivetrain
}
else if(tess_ping >= OBJECT_DISTANCE && tess_drive_state != MOVING)
{
driveForward(-1);
low(LED_1);
}
}
}
void steering::moveServo (int delta, Servo motor ) {
int rd = motor.read();
prt.println(rd);
setServo(delta + rd, motor);
return;
}
void Servo::update(float throttle, float PIDoutput[DIM])
{
servoval[0] =(int)(throttle - PIDoutput[ROLL] - PIDoutput[YAW]);
servoval[1] =(int)(throttle + PIDoutput[ROLL] - PIDoutput[YAW]);
servoval[2] =(int)(throttle - PIDoutput[PITCH] + PIDoutput[YAW]);
servoval[3] =(int)(throttle + PIDoutput[PITCH] + PIDoutput[YAW]);
setServo();
}
/**
* init_module() - This function is called when the module is loaded
*
* Return: On succes the function retuns a 0 (SUCCES). Otherwise a
* negative number is returned.
*/
int init_module(void)
{
Major = register_chrdev(0, DEVICE_NAME, &fops);
if (Major < 0)
{
printk(KERN_ALERT "Registering char device failed with %d\n", Major);
return Major;
}
printk(KERN_INFO "I was assigned major number %d. To talk to\n", Major);
printk(KERN_INFO "the driver, create a device file with\n");
printk(KERN_INFO "'mknod /dev/%s c %d 0'.\n", DEVICE_NAME, Major);
printk(KERN_INFO "Try various minor numbers. Try to echo to\n");
printk(KERN_INFO "the device file.\n");
printk(KERN_INFO "Remove the device file and module when done.\n");
gpio = (volatile unsigned int *)ioremap(GPIO_BASE, 1024);
pwm = (volatile unsigned int *)ioremap(PWM_BASE, 1024);
clk = (volatile unsigned int *)ioremap(CLOCK_BASE, 1024);
// Set pin directions for the output
INP_GPIO(GPIO_OUTP);
OUT_GPIO(GPIO_OUTP);
// PWM and clock settings
SET_GPIO_ALT(GPIO_PWM1, GPIO_ALT);
// stop clock and waiting for busy flag doesn't work, so kill clock
*(clk + CLK_CTL) = CLK_PASSWD | CLK_CTL_KILL;
udelay(10);
// Set clock divider
*(clk + CLK_DIV) = CLK_PASSWD | CLK_DIV_DIVI(96);
// source=osc and enable clock
*(clk + CLK_CTL) = CLK_PASSWD | CLK_CTL_ENAB | CLK_CTL_SRC(CLK_CTL_SRC_OSC);
// disable PWM and start with a clean register
*(pwm + PWM_CTL) = 0;
// needs some time until the PWM module gets disabled, without the delay the PWM module crashes
udelay(10);
// Set the PWM range
*(pwm + PWM_RNG2) = 4000;
// Initialize with a 50% dutycycle
setServo(50);
// start PWM in M/S transmission mode
*(pwm + PWM_CTL) = PWM_CTL_MSEN2 | PWM_CTL_PWEN2;
interrupt_config();
return SUCCESS;
}
void Servo::stopServo()
{
for (int i=0;i<4;i++){
servoval[i]=SERVO_MIN;
};
setServo();
}
asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
{
asynStatus status = setServo(pAxis, 1);
if (asynSuccess != status)
return status;
char cmd[100];
if (velocity != 0)
{
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
sprintf(cmd,"SPA %s 0x50 %f", pAxis->m_szAxisName, velocity);
m_pInterface->sendOnly(cmd);
}
if (pAxis->m_bHasReference)
{
// call FRF - find reference
sprintf(cmd,"FRF %s", pAxis->m_szAxisName);
}
else if (pAxis->m_bHasLimitSwitches)
{
if (forwards)
{
// call FPL - find positive limit switch
sprintf(cmd,"FPL %s", pAxis->m_szAxisName);
}
else
{
// call FNL - find negative limit switch
sprintf(cmd,"FNL %s", pAxis->m_szAxisName);
}
}
else
{
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
return asynError;
}
status = m_pInterface->sendOnly(cmd);
if (asynSuccess != status)
return status;
int errorCode = getGCSError();
if (errorCode == 0)
{
return asynSuccess;
}
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
"PIGCSMotorController::referenceVelCts() failed\n");
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
"PIGCSMotorController::referenceVelCts() failed - GCS Error %d\n",errorCode);
return asynError;
}
int main(int argc, char **argv)
{
// init PWM module for GPIO pin 18 with 50 Hz frequency
initHardware();
// servo test, position in percent: 0 % = 1 ms, 100 % = 2 ms
while (1) {
setServo(0);
sleep(1);
setServo(25);
sleep(1);
setServo(50);
sleep(1);
setServo(75);
sleep(1);
setServo(100);
sleep(1);
}
return 0;
}
void freeRoam() {
while (1) {
writeLed(0xFF);
setMotors(SPEED, SPEED);
_delay_ms(100);
while (1) {
uint16_t dist = readSonarBlocking();
writeLed(dist);
if (dist < STOP_DIST) {
break;
}
// drive
}
setMotors(0, 0);
_delay_ms(200);
uint16_t left, right;
setServo(SERVO0, 0);
_delay_ms(300);
right = readSonarBlocking();
setServo(SERVO0, 180);
_delay_ms(400);
left = readSonarBlocking();
setServo(SERVO0, 90);
_delay_ms(200);
leftEncoder = 0;
rightEncoder = 0;
if(left > right) {
setMotors(-SPEED, SPEED);
} else {
setMotors(SPEED, -SPEED);
}
while (leftEncoder + rightEncoder < ROTATE_BY) {
// rotate
}
setMotors(0, 0);
//_delay_ms(1000);
}
}
void setPose(uint8_t pose)
{
switch (pose)
{
case DRIVE_OPEN:
// set servos to initial positions
setServo(PAN, PAN_CENTER);
setServo(BOOM, MAX_BOOM_UP);
setServo(TILT, TILT_VERT);
setServo(DOOR, DOOR_CLOSED);
myDelay(30);
setServo(LIFT, LIFT_OPEN);
myDelay(20);
disableServo(PAN);
disableServo(BOOM);
disableServo(TILT);
disableServo(DOOR);
disableServo(LIFT);
lcdWriteStr("Drive Open", 0, 0);
break;
case DRIVE_UP:
// set servos to initial positions
setServo(PAN, PAN_CENTER);
setServo(BOOM, BOOM_UP);
setServo(TILT, TILT_VERT);
setServo(DOOR, DOOR_CLOSED);
myDelay(30);
setServo(LIFT, LIFT_UP);
myDelay(50);
disableServo(PAN);
disableServo(BOOM);
disableServo(TILT);
disableServo(DOOR);
disableServo(LIFT);
lcdWriteStr("Drive UP", 0, 0);
break;
default:
lcdWriteStr("ERROR", 0, 0);
break;
}
}
void Servo::init()
{
if(fid_servo==NULL) return;
//initialisation of ESC
for (int i=0;i<4;i++){
servoval[i]=SERVO_MIN;
};
setServo();
sleep(1);
}
void grabBonusBall()
{
// Get in position
//setPose(BB_READY);
setServo(TILT, TILT_BACK);
setServo(LIFT, LIFT_BB_READY);
myDelay(40);
// Drive up to the corner
timer1PWMASet(255);
timer1PWMBSet(255);
forward();
myDelay(20);
brake();
// Execute the pickup sequence
setServo(BOOM, BOOM_BB_GRAB);
myDelay(10);
setServo(TILT, TILT_BB_GRAB);
myDelay(10);
setServo(BOOM, BOOM_UP);
myDelay(15);
setServo(LIFT, LIFT_UP);
myDelay(0);
// Back away from the corner
timer1PWMASet(200);
timer1PWMBSet(200);
reverse();
myDelay(15);
brake();
}
/**
* set motion data to servos according to pre-defined motion command
*
* @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
* @param cmd: motion command
*/
void setMotion(U8 port_id, motionCmd_t cmd)
{
int i;
if (cmd >= sizeof(motionTable)) return;
for (i = 0; i < motionTable[cmd].steps; i++)
{
if (setServo(port_id, motionTable[cmd].pMotion + i) == 0)
{
/* if robot is moving, try the same step again */
if (--i < 0) i = 0;
}
}
}
int main (void) {
uint8_t i = 0;
uint8_t j = 0;
char read = 0;
char command[COMMAND_LENGTH];
uint8_t commandPosition = 0;
uint8_t checksum = 0;
for (i = 0; i < SERVO_COUNT; i++) {
servopos[i] = 100;
}
sei();
servo_init();
ioinit();
while (1) {
read = uart_getchar();
command[commandPosition] = read;
checksum += read;
if (read == 0) {
if (checksum == 0) {
// Handle command
switch (command[0]) {
case 0x01:
setServo(command[1], command[2]);
break;
case 0x02:
getServo(command[1]);
break;
}
} else {
// Command error
uart_putchar(0xFF);
}
checksum = 0;
commandPosition = 0;
} else {
commandPosition++;
}
}
return 0;
}
// 1 = made the transition to zero
u08 decFrameCount()
{
if (0 == frameCount)
{
return 0;
}
frameCount--;
if (frameCount == 0)
{
#ifndef USESTEPPER
setServo(SERVO_STOP);
#else
stepperStop();
#endif // USESTEPPER
waitingFor = FRAMESTOP;
return 1;
}
return 0;
}
// init hardware
void initHardware()
{
// mmap register space
setupRegisterMemoryMappings();
// set PWM alternate function for GPIO18
SET_GPIO_ALT(12, 0);
// stop clock and waiting for busy flag doesn't work, so kill clock
*(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
usleep(10);
// set frequency
// DIVI is the integer part of the divisor
// the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors
// 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz
int idiv = (int) (19200000.0f / 16000.0f);
if (idiv < 1 || idiv > 0x1000) {
printf("idiv out of range: %x\n", idiv);
exit(-1);
}
*(clk + PWMCLK_DIV) = 0x5A000000 | (idiv<<12);
// source=osc and enable clock
*(clk + PWMCLK_CNTL) = 0x5A000011;
// disable PWM
*(pwm + PWM_CTL) = 0;
// needs some time until the PWM module gets disabled, without the delay the PWM module crashs
usleep(10);
// filled with 0 for 20 milliseconds = 320 bits
*(pwm + PWM_RNG1) = 320;
// 32 bits = 2 milliseconds, init with 1 millisecond
setServo(0);
// start PWM1 in serializer mode
*(pwm + PWM_CTL) = 3;
}
asynStatus PIGCSController::initAxis(PIasynAxis* pAxis)
{
// read stage name - to have it in logfile and find
// problems because of mis/non-configured controllers
char cmd[100];
char buf[255];
sprintf(cmd, "CST? %s", pAxis->m_szAxisName);
asynStatus status = m_pInterface->sendAndReceive(cmd, buf, 99);;
if (status != asynSuccess)
{
return status;
}
if (NULL != m_pInterface->m_pCurrentLogSink)
{
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW,
"PIGCSController::initAxis() stage configuration: %s\n", buf);
}
pAxis->m_movingStateMask = int (pow(2.0, pAxis->getAxisNo()) );
return setServo(pAxis, 1);
}
/*
-Pre-Reqs: SetupPWM(uint32_t prescale) with prescale = 10
setupI2C1()
setupUART()
setupADC()
-Globals: sweepspeed
samplespersweep
sweepstartpos
sweepstoppos
-Desc: Moves servo between servo start and end position at a set speed,
scans a set number of times.
If '0' is pressed on the keypad, quits.
*/
void scanMode(void){
initServo(1);
setServo(sweepstartpos);
SystickTimer(100);
// gap between scans = scan range / sample speed
uint32_t scangap = (uint32_t)((float)(sweepstoppos - sweepstartpos)/(float)samplespersweep);
char button;
int turnAmt;
int dir = 1;
int i = samplespersweep - 1;
while(1){
// If servo at edge, change direction
if(servoAtEdge()){
dir = dir*-1;
startSweep();
i = samplespersweep - 1;
averageDistance = _averageSweep();
}
// Turn servo in the "dir" direction by "sweepspeed"
turnAmt = dir*sweepspeed;
turnServo(turnAmt);
if(servo_position % scangap < sweepspeed){
currentIr = irMedian();
sendScanData(currentIr);
sweep[i] = currentIr;
currentRawIr = irMedianRaw();
}
// If 0 was pressed on the keypad, break out of the loop.
button = KeypadTest();
if(button == '0'){
break;
};
SystickTimer(10);
}
}
void setOutputMine(int group, float v){
if(group<numPidMotors){
int val = (int)(v);
val+=127;// center for servos
if (val>255)
val=255;
if(val<0)
val=0;
// if(group == EXTRUDER0_INDEX && !isUpToTempreture()){
// //Saftey so as not to try to feed into a cold extruder
// setServo(group,getServoStop(group),0);
// return;
// }
setServo(group,val,0);
}else{
setHeater( group, v);
}
}
/**
* Start the servo hardware
*/
void initServos(){
SERVO_HW_INIT();
if(getPrintLevel() == NO_PRINT){
CloseUART1();
}
int i;
for(i=0;i<dataTableSize;i++){
pinOff(i);
setServo(i,128,0);
}
// runLinearInterpolationServo(start,stop);
// runSort();
// for(i=0;i<numPidMotor;i++){
// println_I("Servo Positions: ");p_sl_I(getServoPosition(i));
// }
// for(i=0;i<numPidMotor;i++){
// println_I("Sorted Servo Positions index: ");p_sl_I(sort[i]); print_I(" value: ");p_sl_I(position[sort[i]]);
// }
setTimerLowTime();
}
void reset()
{
Serial.println("Reset");
#ifndef USESTEPPER
setServo(SERVO_STOP);
#else
stepperInit();
#endif // USESTEPPER
lampOff();
//SHUTTERINT_OFF;
CAMERA_OFF;
SHUTTER_CLOSE;
setMotor(MOTOR_OFF);
//motorRewind = MOTOR_OFF;
parameter = 0;
waitingFor = NONE;
verbose = 0;
frameCount = 0;
//optoIntTimeout = 0;
shutterState = 0;
#ifdef USEFRAM
framIndex = 0;
#endif // USEFRAM
}
