void __encoder_read(SENSOR* sensor){ ENCODER_TYPE ticks; ENCODER* encoder = (ENCODER*)sensor; TIMER_SNAPSHOT lastTime; TIMER_SNAPSHOT prevTime; // Disable interrupts whilst reading counter. CRITICAL_SECTION{ ticks = encoder->counter; if(encoder->interpolate){ lastTime = encoder->snapshot[encoder->tickIndex]; prevTime = encoder->snapshot[encoder->tickIndex ^ 1]; } } if(encoder->interpolate){ TICK_COUNT lastTick,prevTick,now; now = clockGetus(); lastTick = timerSnapshotToTicks(&lastTime); prevTick = timerSnapshotToTicks(&prevTime); encoder->timeSinceLastTick = now - lastTick; // us since last tick encoder->durationOfLastTick = lastTick - prevTick; // us between last two ticks (ie speed) } if(encoder->inverted){ ticks *= -1; } // Store the last read value encoder->value=ticks; }
uint8_t marqueeSendByte(MARQUEE* marquee, uint8_t byte){ marquee_init(marquee); if(marquee->txt){ if(byte=='\n'){ // Start writing at the beginning of the line marquee->txt[marquee->writePos] = '\0'; marquee->writePos = 0; CRITICAL_SECTION_START; if(!marquee->active){ marquee->active = TRUE; marquee->blink = FALSE; marquee->readPos=0; scheduleJob(&marqueeUpdate,(SchedulerData)marquee,clockGetus(),marquee->delayChar); } CRITICAL_SECTION_END; }else if(byte!='\r'){ // Now put the character to the buffer if(marquee->writePos < marquee->txtSize){ char* put = marquee->txt + marquee->writePos; CRITICAL_SECTION_START; *put++ = byte; *put = '\0'; marquee->writePos += 1; marquee->readPos = 0; CRITICAL_SECTION_END; } } } return byte; }
// Update the PID with the actual position and return the new output float pidSetActual(PID* pid,float actual){ // Find time difference since last call TICK_COUNT dt; if(g_heartbeat){ // We have got a timer TICK_COUNT now = clockGetus(); dt = now - pid->lastTime; pid->lastTime = now; }else{ // There is no timer - so assume called every 10ms dt = (TICK_COUNT)10000; } float target; CRITICAL_SECTION{ target = pid->setPoint; } float error = target - actual; //find error //if circular control system, check for and handle wrap around if (pid->isCircular){ float spanDiv2 = pid->inSpan / 2; if (error < -spanDiv2){ error += pid->inSpan; } if (error > spanDiv2){ error -= pid->inSpan; } } pid->prevError = error; // save error for next go around //sum the errors for integral and clamp to limit pid->integral += error; pid->integral = CLAMP( pid->integral, -(pid->iLimit), pid->iLimit); //find slope of error over time float derivative = (error - pid->prevError) / dt; float out = (pid->kP * error) + (pid->kI * pid->integral) + (pid->kD * derivative); //compare output to limits out = CLAMP(out, pid->outMin, pid->outMax); return out; }
// Initialise the software TICK_COUNT appInitSoftware(TICK_COUNT loopStart){ for (int i=0; i < 14; i+=1){ led[i].r = 0; led[i].g = 0; led[i].b = 0; } setLeds(); halfperiods[0] = 500000; // us halfperiods[1] = 625000; switchtime = clockGetus(); newbrightness = minbright; return 0; }
uint8_t marqueeSendByte(MARQUEE* marquee, uint8_t byte){ marquee_init(marquee); if(marquee->txt){ if(byte=='\n'){ // Start writing at the beginning of the line marquee->txt[marquee->writePos] = '\0'; marquee->writePos = 0; CRITICAL_SECTION{ if(!marquee->active){ marquee->active = TRUE; marquee->blink = FALSE; marquee->readPos=0; scheduleJob(&marqueeUpdate,(SchedulerData)marquee,clockGetus(),marquee->delayChar); } } }else if(byte!='\r'){
// Start running a new animation void gaitRunnerPlay(G8_RUNNER* runner, uint8_t animation, int16_t loopSpeed, DRIVE_SPEED speed, int16_t repeatCount){ // Update variables with interrupts off - in case the gait is // updated under interrupts TICK_COUNT now = clockGetus(); CRITICAL_SECTION { runner->animation = animation; runner->repeatCount = repeatCount; runner->frame = 0; runner->playing = TRUE; runner->startTime = now; runner->currentTime = (speed<0) ? loopSpeed : 0; runner->totalTime = loopSpeed; runner->speed = speed; runner->backwards = FALSE; } // Set servos to initial position gaitRunnerProcess(runner); }
// Update the gait runner and move servos to new positions // Call it from your main loop or via the scheduler to do it in the background // NB There is no point scheduling any faster than 20ms as that is the servo refresh rate // Return true if an animation is playing boolean gaitRunnerProcess(G8_RUNNER* runner){ if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){ return FALSE; } TICK_COUNT now = clockGetus(); int16_t interval = (now - runner->startTime)>>16; if(interval == 0){ return TRUE; } // There has been a noticeable change in time runner->startTime = now; if(runner->backwards){ interval *= -1; } interval *= runner->speed; // Re-check as drive speed could be zero if(interval == 0){ return TRUE; } // Locate the current animation const G8_ANIMATION* animation = &runner->animations[runner->animation]; // Update the current time with the new interval int16_t currentTime = runner->currentTime + interval; if(currentTime >= runner->totalTime){ // We have finished playing the animation if(pgm_read_byte(&animation->sweep)==FALSE){ currentTime %= runner->totalTime; // Set back to start of loop if(runner->repeatCount){ runner->repeatCount -= 1; // One less frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to final position } } }else{ // Start going backwards through the animation currentTime = runner->totalTime - (currentTime - runner->totalTime); runner->backwards = TRUE; } }else if(currentTime < 0){ // We have moved before the start if(pgm_read_byte(&animation->sweep)==FALSE){ currentTime = runner->totalTime + currentTime; if(runner->repeatCount){ runner->repeatCount += 1; // One more frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to start position } } }else{ // We have completed a sweep runner->backwards = FALSE; currentTime = -currentTime; if(runner->repeatCount){ runner->repeatCount -= 1; // One less frame to go if(runner->repeatCount==0){ runner->playing = FALSE; // we have reached the end currentTime = 0; // set servos to initial position } } } } runner->currentTime = currentTime; // range is 0....totalTime // Current time in the range 0...SCALE_X uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X); uint16_t frameStartTime = 0; uint16_t frameEndTime = SCALE_X; // Locate the correct frame const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames); uint8_t i; for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){ const G8_FRAME* f = &frame[i]; frameStartTime = pgm_read_word(&f->time); if(frameStartTime <= frameTime){ frame = f; break; } frameEndTime = frameStartTime; frameStartTime = 0; } runner->frame = i; #ifdef DEBUG rprintf("\n%u,%d",i,currentTime); #endif // Now have:- frameStartTime <= frameTime <= frameEndTime // We now need to find the distance along the curve (0...1) that represents // the x value = frameTime // First guess from 0..1 uint16_t frameTimeOffset = frameTime-frameStartTime; double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime)); const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs); for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){ double distanceMin = 0.0; double distanceMax = 1.0; double distance = distanceGuess; // Find the correct distance along the line for the required frameTime for(uint8_t iterations=0; iterations<20; iterations++){ uint16_t actualX = calcX(limb, distance); if(actualX == frameTimeOffset) break; // Found it if( actualX < frameTimeOffset){ // We need to increase t distanceMin = distance; }else{ distanceMax = distance; } // Next guess is half way between distance = distanceMin + ((distanceMax - distanceMin) / 2); } // We now know the distance runner->speeds[l] = calcY(limb,distance); #ifdef DEBUG rprintf(",%d",speed); #endif } // next limb #ifndef DEBUG // Set all the servo speeds in quick succession for(uint16_t l = 0; l < runner->num_actuators; l++){ __ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]); int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]); speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX); __act_setSpeed(servo,(DRIVE_SPEED)speed); } #endif return gaitRunnerIsPlaying(runner); }
// This is the main loop TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) { //Toggle mode via button if (!buttonWait){ if(SWITCH_pressed(&button)){ // pressed mode = (mode + 1) % NUMMODES; buttonWait = 1; buttonWaitStartTime = loopStart; // Clear vals for new mode for (int i=set; i < 14; i++){ led[i].r = 0; led[i].g = 0; led[i].b = 0; } setLeds(); } } // Wait 1 second after noticing a button press before allowing another button press else { if (clockHasElapsedGetOverflow(buttonWaitStartTime, buttonWaitTime, &unusedButtonRemainingTime)){ buttonWait = 0; } } /// alternating half bright half fading if (mode == 1) { if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) { // rprintf("boom!\n"); delay_ms(400); // Switch modes if we've reached the end of increasing brightness if (dir == 1){ set = (set + 1) % 2; } switchtime = clockGetus(); dir *= -1; remainingtime = halfperiods[set] - remainingtime; // Set half the LEDs to maximum brightness for (int i=(1-set); i < 14; i+=2){ // led[i].b = maxbright * 2; led[i].r = maxbright * 2; led[i].g = maxbright; } } // Calculate brightness of dimmer LEDs if(dir == 1){ // increasing brightness newbrightness = maxbright - (maxbright - minbright) * remainingtime / halfperiods[set]; } else{ // decreasing brightness newbrightness = minbright + (maxbright - minbright) * remainingtime / halfperiods[set]; } // Monocolor // newbrightness *= 2; // newbrightness += 1; for (int i=set; i < 14; i+=2){ // blue // led[i].b = newbrightness; // orange led[i].r = newbrightness * 2 + 1; led[i].g = newbrightness + 1; } setLeds(); } /// else if (mode == 2) { if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) { // rprintf("boom!\n"); set = (set + 1) % 2; switchtime = clockGetus(); remainingtime = halfperiods[set] - remainingtime; } for (int i=0; i < 14; i+=2){ led[i].b = 0; } j = (14 * (remainingtime /1000)) / (halfperiods[set] / 1000); led[j].b = maxbright; setLeds(); } /// moving orange else if (mode == 3) { if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) { //delay_ms(250); // Switch modes if we've reached the end of increasing brightness //if (dir == 1){ set = (set + 1) % 2; //} switchtime = clockGetus(); //dir *= -1; remainingtime = halfperiods[set] - remainingtime; } for (int i=0; i < 14; i++){ led[i].r = 0; led[i].g = 0; led[i].b = 0; } j = (14 * (remainingtime /1000)) / (halfperiods[set] / 1000); led[j].r = maxbright; led[j].g = maxbright/2; //led[j].b = maxbright; setLeds(); } /// FLASHING WHITE else if (mode == 4) { if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) { delay_ms(400); // Switch modes if we've reached the end of increasing brightness if (dir == 1){ set = (set + 1) % 2; } switchtime = clockGetus(); dir *= -1; remainingtime = halfperiods[set] - remainingtime; } // Calculate brightness if(dir == 1){ // increasing brightness newbrightness = maxbright - (maxbright - minbright) * remainingtime / halfperiods[set]; } else{ // decreasing brightness newbrightness = minbright + (maxbright - minbright) * remainingtime / halfperiods[set]; } for (int i=0; i < 14; i++){ led[i].r = newbrightness * 2; led[i].g = newbrightness * 2; led[i].b = newbrightness * 2; } setLeds(); } else if (mode == 5) { k = (k+1)%w; y = 4; z = 7; w = 7; int val = (k*y)%z; // >>> for k in xrange(w): print (k*y)%z; // for (int k=0; k < w; k+=x) // { // val = (k*y)%z; // } for (int i=0; i < 14; i++){ led[i].r = maxbright * 2; led[i].g = maxbright; // led[i].b = newbrightness * 2; } led[val].r = maxbright * 2; led[val].g = maxbright * 2; led[val+7].r = maxbright * 3; led[val+7].g = maxbright * 2; setLeds(); return 500000; } // /////// // // BlinkM control (doesn't work; probably need pull up resistors) // uint8_t blinkm_packet_setcolor[4]; // blinkm_packet_setcolor[0] = (uint8_t)'n'; // blinkm_packet_setcolor[1] = 50; // blinkm_packet_setcolor[2] = 50; // blinkm_packet_setcolor[3] = 50; // i2cMasterSend(&blinkm.i2cInfo, sizeof(blinkm_packet_setcolor), blinkm_packet_setcolor); // // i2cMasterSend(&blinkm.i2cInfo, sizeof(blinkm_packet_setcolor), *blinkm_packet_setcolor); // delay_ms(50); else if (mode == 0){ for (int i=0; i < 14; i++){ led[i].r = 0; led[i].g = 0; led[i].b = 0; } setLeds(); } return 20000; }
// This routine is called repeatedly - its your main loop TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){ TICK_COUNT frameEnd = frameStart + 20000; wdt_reset(); // very occasionally RF module misses interrupt. // this usually happens when signal is weak. // testing interrupt pin manually here is an effective workaround. //if (pin_is_low(E7)){ // cyrfInterupt(&cyrf_0); //} //cyrf6936_RX_mode(&cyrf_0); // poll sensors as often as possible // get readings from A2D. //int tmp = a2dConvert8bit(ADC_CH_ADC6); // = ; currDraw += a2dConvert10bit(ADC_CH_ADC1); rcAccelX += a2dConvert10bit(ADC_CH_ADC7); rcAccelY += a2dConvert10bit(ADC_CH_ADC6); rcAccelZ += a2dConvert10bit(ADC_CH_ADC5); rcGyroX += a2dConvert10bit(ADC_CH_ADC4); rcGyroY += a2dConvert10bit(ADC_CH_ADC3); sensorCount++; // keep track of number of times ADC has been read. int tmp; if(frameEnd <= clockGetus()) { frameStart = frameEnd; // this section happens every 20ms frame. // frame counter. data[RX_FRA_NUM]++; data[RX_BAT_CUR] = (currDraw / sensorCount) >>2; // average accelerometer. RAcc1.RXACC = (rcAccelX / sensorCount) -AccOffset; RAcc1.RYACC = (rcAccelY / sensorCount) -AccOffset; RAcc1.RZACC = (rcAccelZ / sensorCount) -AccOffset; RAccSize1 = sqrt((RAcc1.RXACC*RAcc1.RXACC)+(RAcc1.RYACC*RAcc1.RYACC)+(RAcc1.RZACC*RAcc1.RZACC)); // extract roll and pitch from accelerometer's 3 axis. data[RX_ACCEL_Y] = AtAcc.AXZ = 90*atan2(RAcc1.RXACC,RAcc1.RZACC) + data[RX_AUTOP_Y_TRIM] - 0x7F; data[RX_ACCEL_Y] += 0x7F; data[RX_ACCEL_X] = AtAcc.AYZ = 90*atan2(RAcc1.RYACC,RAcc1.RZACC) + data[RX_AUTOP_X_TRIM] - 0x7F; data[RX_ACCEL_X] += 0x7F; // *** experimental *** // multiply accelerometer readings by a factor of current draw. data[RX_ACCEL_Y] = AtAcc.AXZ = AtAcc.AXZ - data[RX_BAT_CUR]/4.5; data[RX_ACCEL_Y] += 0x7F; data[RX_ACCEL_X] = AtAcc.AYZ = AtAcc.AYZ - data[RX_BAT_CUR]/5.5; data[RX_ACCEL_X] += 0x7F; // average gyro. int x_gyro_offset = data[RX_GYRO_X_OFFSET_L] + (0x100*data[RX_GYRO_X_OFFSET_L]); int y_gyro_offset = data[RX_GYRO_Y_OFFSET_L] + (0x100*data[RX_GYRO_Y_OFFSET_L]); data[RX_GYRO_X] = AtGyr.AYZ = (rcGyroX / sensorCount); data[RX_GYRO_X] += 0x7F - x_gyro_offset; data[RX_GYRO_Y] = AtGyr.AXZ = (rcGyroY / sensorCount); data[RX_GYRO_Y] += 0x7F - y_gyro_offset; // work out estimated angle based on previous estimate and sensor data. data[RX_EST_X] = AtEst1.AYZ = (((AtEst0.AYZ + ((AtGyr.AYZ - x_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AYZ)) / (1 + GyroBias); data[RX_EST_X] += 0x7F; data[RX_EST_Y] = AtEst1.AXZ = (((AtEst0.AXZ + ((AtGyr.AXZ - y_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AXZ)) / (1 + GyroBias); data[RX_EST_Y] += 0x7F; // something mental is going on at power on so stop excessive values. if(AtEst1.AYZ > (0x2A7FF + 90)){ AtEst1.AYZ = (0x2A7FF + 90);} if(AtEst1.AYZ < 0x2A7FF - 90){ AtEst1.AYZ = (0x2A7FF - 90);} if(AtEst1.AXZ > (0x38DFF + 90)){ AtEst1.AXZ = (0x38DFF + 90);} if(AtEst1.AXZ < 0x38DFF - 90){ AtEst1.AXZ = (0x38DFF - 90);} //data[RX_EST_X] = data[RX_ACCEL_X]; //data[RX_EST_Y] = data[RX_ACCEL_Y]; //rprintfu16((int)(AtGyr.AXZ/0x10000) + 0x8000); //rprintfu16(AtGyr.AXZ ); //rprintfProgStrM("\n"); currDraw = rcAccelX = rcAccelY = rcAccelZ = rcGyroX = rcGyroY = 0; AtEst0 = AtEst1; sensorCount1=sensorCount; sensorCount=0; /* // read battery and current sensor tmp = a2dConvert8bit(ADC_CH_ADC0); if(tmp > data[RX_BAT_VOLT]){ data[RX_BAT_VOLT]++; } else if(tmp < data[RX_BAT_VOLT]){ data[RX_BAT_VOLT]--; } data[RX_BAT_CUR] = a2dConvert8bit(ADC_CH_ADC1); */ if(!(testIfPacketReceived(data)==0)){ // this section done if a packet has been received this frame. // (the data[] should have arrived during an interrupt on the RF module IRQ pin.) uptime++; // set servo positions. set_servos(data); pin_high(C1); // warning LED off // watch for PS2 controller button presses. // these are de-bounced by running a simple counter. uint16_t tmp = ((data[TX_PS2_0] <<8) + data[TX_PS2_1]); uint8_t i; for (i=0; i<16; i++){ if(tmp & (1<<i)){ // button pressed. increase counter //rprintfProgStrM("\nPressed:"); rprintfu08(i); if (buttonPressed[i] < 0xFF){ buttonPressed[i]++; } } else if(buttonPressed[i] > 0){ // button has just been released. save value in counter to de-bounce buttons. //rprintfProgStrM("\nReleased:"); rprintfu08(i); buttonReleased[i]=buttonPressed[i]; buttonPressed[i]=0; } else{ buttonReleased[i]=0; buttonPressed[i]=0; } } if (buttonReleased[PS2_B_BTN_TRIANGLE] > 5){ set_trims(data); } if (buttonReleased[PS2_B_BTN_CIRCLE] > 5){ reset_trims(); } if (buttonReleased[PS2_B_BTN_X] > 5){ data[RX_GYRO_X_OFFSET_H] = (uint8_t)(AtGyr.AYZ / 0x100); data[RX_GYRO_X_OFFSET_L] = (uint8_t)(AtGyr.AYZ - (AtGyr.AYZ*0x100)); data[RX_GYRO_Y_OFFSET_H] = (uint8_t)(AtGyr.AXZ / 0x100); data[RX_GYRO_Y_OFFSET_L] = (uint8_t)(AtGyr.AXZ - (AtGyr.AXZ*0x100)); eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_H], data[RX_GYRO_X_OFFSET_H]); eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_L], data[RX_GYRO_X_OFFSET_L]); eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H], data[RX_GYRO_Y_OFFSET_H]); eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L], data[RX_GYRO_Y_OFFSET_L]); } if (buttonReleased[PS2_B_DPAD_UP] > 5){ if (buttonPressed[PS2_B_BTN_R1] > 0){ if(buttonPressed[PS2_B_BTN_R2] > 0){ //increase autopilot sensitivity (Y axis). data[RX_AUTOP_Y_MUL] ++; } else{ //increase autopilot trim (Y axis). data[RX_AUTOP_Y_TRIM] ++; } } else if (data[RX_SERVO_2_MUL] < 137){ data[RX_SERVO_2_MUL]++; } } if (buttonReleased[PS2_B_DPAD_DOWN] > 5){ if (buttonPressed[PS2_B_BTN_R1] > 0){ if(buttonPressed[PS2_B_BTN_R2] > 0){ if (data[RX_AUTOP_Y_MUL] > 2){ //decrease aoutopilot sensitivity (Y axis). data[RX_AUTOP_Y_MUL] --; } } else{ //decrease autopilot trim (Y axis). data[RX_AUTOP_Y_TRIM] --; } } else if (data[RX_SERVO_2_MUL] > 117){ data[RX_SERVO_2_MUL]--; } } if (buttonReleased[PS2_B_DPAD_RIGHT] > 5){ if (buttonPressed[PS2_B_BTN_R1] > 0){ if(buttonPressed[PS2_B_BTN_R2] > 0){ //increase aoutopilot sensitivity (X axis). data[RX_AUTOP_X_MUL] ++; } else{ //increase autopilot trim (X axis). data[RX_AUTOP_X_TRIM] ++; } } else if (data[RX_SERVO_4_MUL] < 137){ data[RX_SERVO_4_MUL]++; data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL]; } } if (buttonReleased[PS2_B_DPAD_LEFT] > 5){ if (buttonPressed[PS2_B_BTN_R1] > 0){ if(buttonPressed[PS2_B_BTN_R2] > 0){ if (data[RX_AUTOP_X_MUL] > 2){ //decrease autopilot sensitivity (X axis). data[RX_AUTOP_X_MUL] --; } } else{ //decrease autopilot trim (X axis). data[RX_AUTOP_X_TRIM] --; } } else if (data[RX_SERVO_4_MUL] > 117){ data[RX_SERVO_4_MUL]--; data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL]; } } } else{ // no packet received this frame pin_low(C1); // warning LED on } // enable receive mode. cyrf6936_RX_mode(&cyrf_0); cyrf6936_RX_mode(&cyrf_1); if(data[RX_PAC_GAP] > 25){ // no data received for 1/2 second // so clear PS2 controller readings // and put servos to default positions. if(data[TX_CONTROLER]==0){ data[TX_PS2_LY] = 128; } else{ data[TX_PS2_LY] = 254; } data[TX_PS2_LX] = 128; data[TX_PS2_RY] = 128; data[TX_PS2_RX] = 128; set_servos(data); //pin_low(C0); // warning LED on } else{ //pin_high(C0); // warning LED off } // debug status LED off pin_high(C0); }
// This routine is called once to allow you to set up any other variables in your program // You can use 'clock' function here. // The loopStart parameter has the current clock value in μS TICK_COUNT appInitSoftware(TICK_COUNT loopStart){ act_setSpeed(&servo_0,DRIVE_SPEED_CENTER); act_setSpeed(&servo_1,DRIVE_SPEED_CENTER); act_setSpeed(&servo_2,DRIVE_SPEED_CENTER); act_setSpeed(&servo_3,DRIVE_SPEED_CENTER); act_setSpeed(&servo_4,DRIVE_SPEED_CENTER); act_setSpeed(&servo_5,DRIVE_SPEED_CENTER); // starting values for trims. data[RX_SERVO_0_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CH]); data[RX_SERVO_0_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CL]); data[RX_SERVO_1_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CH]); data[RX_SERVO_1_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CL]); data[RX_SERVO_2_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CH]); data[RX_SERVO_2_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CL]); data[RX_SERVO_3_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CH]); data[RX_SERVO_3_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CL]); data[RX_SERVO_4_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CH]); data[RX_SERVO_4_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CL]); data[RX_SERVO_0_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_MUL]); data[RX_SERVO_1_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_MUL]); data[RX_SERVO_2_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_MUL]); data[RX_SERVO_3_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_MUL]); data[RX_SERVO_4_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_MUL]); data[RX_AUTOP_X_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_MUL]); data[RX_AUTOP_Y_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_MUL]); data[RX_AUTOP_X_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_TRIM]); data[RX_AUTOP_Y_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_TRIM]); data[RX_GYRO_X_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_L]); data[RX_GYRO_X_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_H]); data[RX_GYRO_Y_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L]); data[RX_GYRO_Y_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H]); if ((data[RX_SERVO_1_CH] < 0x60) | (data[RX_SERVO_1_CH] > 0x80)){ reset_trims(); } if ((data[RX_SERVO_3_MUL] > 137) || (data[RX_SERVO_3_MUL] < 117)){ data[RX_SERVO_3_MUL] = 127;} if ((data[RX_SERVO_4_MUL] > 137) || (data[RX_SERVO_4_MUL] < 117)){ data[RX_SERVO_4_MUL] = 127;} // initialise RF module. cyrf6936_Initialise_soft(&cyrf_0); cyrf6936_Initialise_soft(&cyrf_1); // enable interrupt on pin connected to cyrf6936 interrupt pin. cyrfIntEnable(); frameStart=clockGetus(); data[RX_BAT_VOLT] = a2dConvert8bit(ADC_CH_ADC0); AtEst1.AYZ = AtEst0.AYZ = 0x2A7FF; AtEst1.AXZ = AtEst1.AXZ = 0x38DFF; return 0; // don't pause after }