Post on 04-May-2018
ContentsIntroduction.................................................................................................................................................3
Objective.....................................................................................................................................................3
KHR-1 Background Information...................................................................................................................3
Reverse Engineering the KHR-1 Servo Control Signal..................................................................................4
Choosing a Microcontroller.........................................................................................................................4
Controller:...........................................................................................................................................5
Controller Board..........................................................................................................................................5
Software......................................................................................................................................................5
Software development steps...............................................................................................................5
Software Overview (better-walk.c)..............................................................................................................6
Final Result..................................................................................................................................................7
Proposed Changes / Future Improvements.................................................................................................7
“better_fwd_walk_data.csv” in parseable array.........................................................................................8
Xmega-blinkled.c.........................................................................................................................................9
Stepping-Servo-Control.c...........................................................................................................................10
Better-walk.c (final example program)......................................................................................................14
KHR-1 Notes (initial development notes)..................................................................................................21
Page 2
IntroductionA robot is a mechanical or virtual intelligent agent that can perform tasks automatically or with guidance, typically by remote control. They have been developed in for many purposes, primarily, to create machines that could operate autonomously, reduce human risk and most notably to perform sometimes complex, repetitive tasks much more quickly. In this project we attempt to use the KHR-1 robot to perform task. We have tried to use the physical robot to perform such tasks but we ended up using the simulator.
ObjectiveBecause of the previous failure to operate, control and henceforth improve upon the existing KHR-1 robots, a new design was to be implemented for the command and control interface. With this objective in mind, further extensibility was also required to connect the robot to the P.A.V.E. site for live demoing and control of the Intelligent Robotics Laboratory Robot Theatre.
KHR-1 Background InformationTo start off, the KHR-1 servo control mechanism needed to be reverse engineered before significant progress could be made in choosing and implementing a microcontroller. The first task was to identify the number of channels and operating region of the servos. The KHR-1 uses 17 servos, giving seventeen degrees of freedom. With the robot used in development, additional servos were attached to the upper section of each arm, in parallel with CH2 and CH8, in order to give 19 degrees of freedom, more human-like operation and better control of the arms. Limited documentation was give as far as circuits and technical information about control systems of the KHR-1 robot but searches provided the following technical documentation on the KRS-786 ICS servo-motor:
Double-sided horn 180˚ gear limit 8.7 kg/cm torque 60˚ @ 0.17 second radial travel 6V operation 45 grams
Page 3
Reverse Engineering the KHR-1 Servo Control SignalKondo provided no further technical data on the operation of the servo-motor but from previous experience, servos are traditionally controlled by a 100Hz-2.5kHz PWM signal. Knowing this and the operating voltage, a Tektronix Arbitrary Waveform Generator was used to scan this operating region. The servo-motor only gave a response at 200Hz. Initial tests also indicated an extremely wide logic level range of the PWM signal, able to operate as low as 1.5 volts peak-to-peak with an offset half of the peak voltage.
Knowing the voltage proved that all development boards on hand could be used to generate the proper pulse width modulation to control the KRS-786 servo-motor but the correct range then had to be identified. Using the 200Hz waveform, simple math shows that the pulse period is 5 milliseconds with this information, the Tektronix Arbitrary Waveform Generator was used to sweep from 0-5 milliseconds and to take input from a digital ammeter in order to automate the task find the full operating range of the PWM signal. Stepping in 10us intervals, the arbitrary waveform generator identified the operating range of the PWM to be between 700us and 2.3ms, representing the low and the high positions, respectively.
Choosing a MicrocontrollerBecause of the time constraints imposed upon the extension of this project, only controllers on hand were to be considered in developing the new controller.
Atmel ATxmega128A3 Atmel ATmega168A Atmel ATmega328P Mars MR7910B-0A Integrated KHR-1 PIC
With such a selection, consideration had to be give to the task at hand. Since the KHR-1 needed 2 controllers to operate with the integrated PIC, programming in parallel or a master/slave model was going to waste much time in development. Further, because many of the boards could not be verified to be fully functioning, they were almost immediately eliminated from the decision matrix. Next, was the Mars microcontroller, being an older, non-US microcontroller, the documentation was all in Chinese and was picked up as a hobby and alternative to the Atmel ATxmega series microcontrollers. This microcontroller was also eliminated due to time of development but also because of non-english documentation. This left the 3 Atmel microcontrollers. Reviewing the datasheets, it was found the clock source on the ATmega microcontrollers needed a clock source divided by a static integer. Since the only available clock sources were 16MHz (crystal), 10MHz(crystal), 8MHz (internal RC), 32.2kHz
Page 4
(internal RC), it was determined that a prescaler could not be selected to output a 200MHz without using the fast-PWM mode. Even then, there were only 3 timers available on both the ATmega controllers and there were 17 servos on the KHR-1. This left the ATxmega128A3, with a total of seven, 16-bit timer/counters, four controlling four pins and three controlling two pins.
Controller: Boston Android ATxmega128A3 http://www.bostonandroid.com/EVAL-USB-128-Lite.html
Controller BoardAmong the most important physical consideration of the redesign of the KHR-1 controller, there needed to be enough pins for servo control, input for power and a reasonable programming interface. With that in mind, a proto board was cut to the dimensions of the rear of the KHR-1. A total of 30 pins were connected for the input and then the microcontroller was hooked up with female headers for faster integration and swapping from the robot.
SoftwareWith a functional proto-board, it was now time to enter the software portion of the controller redesign. It should be noted, the software developed is only a proof of concept and should be used by example only. Not all servos were used in the software development. Henceforth, the development was purely for reference and example to aid future development of the redesigned KHR-1 robot.
Software development steps Blink LED L on ATxmega128A3 Initialize Timer/counter with OSU-Tekbot header (courtesy Edward Tanous at Tektronix, B.S.-
Electrical Engineering) Prototype PWM and experiment with servo control Implement multiple servo control Prototype transformation function (to use GA output tables)
Page 5
Implement transform function to provide full example of making the KHR-1 walk
For this, AVR-Studio 5 was used, headphones were found, a combination of trance and dubstep was selected and software development commenced. The result is currently available in the project wiki, hosted at https://projects.cecs.pdx.edu as well as some code displayed below.
Software Overview (better-walk.c)1. Set F_CPU – for timer delay2. Constants definitions3. Include:
a. Stdio – standard I/O opsb. Avr/io – avr specific I/O opsc. Delay – delay headerd. String – c/lib string functions
4. pwmTimerInit()a. initializes pulse width modulation
5. moveServo(channel, position)a. correct input postion for controller to utilizeb. set TC compare (per channel)
6. robotPosition(positions[], speed)a. parse positionb. compare to new positionc. while servo hasn’t moved
i. update position7. main()
a. take in commandsb. init PWMc. while (true)
i. take in commands (from above table)ii. execute commands
Page 6
Final Result
Proposed Changes / Future ImprovementsWhile providing documentation and example code, not all servos were prototyped. Currently, only the legs of the robot are hooked up to the microcontroller. It is recommended the control board be rebuilt with the following changes:
Connect all servos and implement better _walk.c to control all 17 servos 6V Switching voltage regulator and supporting circuitry for input Proto-board with Copper plated vias arms connected to timer/counter capable pins (Port E?)
Page 7
Power to microcontroller Vin from servo power line Better common ground (currently using jumper wire) Drill mounting holes for use on backside of KHR-1 Li-Po battery pack (a 4.5v was used to test capability, 6v recommended) Utilize 16Mhz or 32Mhz clock rate (software, requires timing adjustment (clock div = 128?)) Port PAVE arduino code to AVR-C to participate in robot theatre. Pan/tilt laser targeting system
“better_fwd_walk_data.csv” in parseable arrayThe first column is the speed, subsequent columns are the position of each servo channel, 1-24.
{4,0,1,0,225,225,0,0,1,0,225,225,225,0,-35,-52,21,15,225,0,35,52,-21,15,225},
{4,0,1,0,225,225,-30,0,1,0,225,225,225,-11,-35,-52,21,4,225,-12,35,53,-21,4,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-10,-75,-105,50,5,225,-9,75,60,-27,5,225},
{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},
{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},
{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},
{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},
{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},
{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},
{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},
{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},
{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},
{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},
{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},
{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},
Page 8
{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},
{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},
{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},
{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},
{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},
{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},
{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},
{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},
{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},
{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},
{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},
{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},
{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},
{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},
{3,0,1,0,225,225,0,0,1,0,225,225,225,-10,-45,-60,18,5,225,-9,45,53,-21,5,225},
{5,0,1,0,225,225,0,0,1,0,225,225,225,0,-35,-52,21,15,225,0,35,52,-21,15,225}
Xmega-blinkled.c#include <stdio.h>#include <avr\io.h>
void Config32MHzClock(void);
int main(void){ Config32MHzClock(); // configure sysclk=32MHz RC oscillator
Page 9
CLK.PSCTRL = 0x00; // no division on peripheral clock
// make clkout on PORTE:7 PORTCFG.CLKEVOUT = PORTCFG_CLKOUT_PE7_gc; PORTE.DIR = (1<<7); // clkout
// configure timer/counter0 TCC0.CTRLA = 0x7; // clk/1024
// configure PORTF:0 as output to LED PORTF.DIR=(1<<0); // Eval-01 64A3 RevA, Eval-USB// PORTF.DIR=(1<<2); // Eval-01 128A3,64A3 RevB
// blink LED at 2Hz while(1) { PORTF.OUT ^= (1<<0); // Eval-01 64A3 RevA, Eval-USB// PORTF.OUT ^= (1<<2); // Eval-01 128A3, 64A3 RevB while(TCC0.CNT < 7812) // roughly 250ms
asm("nop"); TCC0.CNT=0; // reset };
return 0;};
void Config32MHzClock(void){ CCP = CCP_IOREG_gc; //Security Signature to modify clock // initialize clock source to be 32MHz internal oscillator (no PLL) OSC.CTRL = OSC_RC2MEN_bm; // enable internal 32MHz oscillator while(!(OSC.STATUS & OSC_RC2MRDY_bm)); // wait for oscillator ready CCP = CCP_IOREG_gc; //Security Signature to modify clock CLK.CTRL = 0x01; //select sysclock 32MHz osc};
Stepping-Servo-Control.c#define F_CPU 2000000UL //because i'm lazy...#define TCPeriod 1250 //Timer/Counter Period#define UpperServoLimit 575 //upper servo range limit with 1250 TC Period#define LowerServoLimit 175 //lower servo range limit with 1250 TC Period#define HalfServoLimit 375 //half servo range with 1250 TC Period#define tdelay 6 //quick edit delay time#include <stdio.h>#include <avr\io.h>
Page 10
#include <util\delay.h>#include "TC_driver.h"
void Config32MHzClock(void);
/* assumes F_CPU is set to correct frequency! */void delayms(uint16_t ms){
while (ms){
_delay_ms(1);ms--;
}}
void timer_init(){
/* Enable output on pins 0-5 */ PORTD.DIRSET = 63; PORTC.DIRSET = 63;
/* Set the TC period. */ TC_SetPeriod( &TCD0, TCPeriod );
TC_SetPeriod( &TCD1, TCPeriod ); TC_SetPeriod( &TCC0, TCPeriod );
TC_SetPeriod( &TCC1, TCPeriod );
/* Configure the TC for single slope mode. */TC0_ConfigWGM( &TCD0, TC_WGMODE_SS_gc );
TC0_ConfigWGM( &TCC0, TC_WGMODE_SS_gc ); TC1_ConfigWGM( &TCD1, TC_WGMODE_SS_gc ); TC1_ConfigWGM( &TCC1, TC_WGMODE_SS_gc );
/* Enable Compare channel A,B on timer C1 and D1 */ TC1_EnableCCChannels( &TCD1, TC1_CCAEN_bm|TC1_CCBEN_bm); TC1_EnableCCChannels( &TCC1, TC1_CCAEN_bm|TC1_CCBEN_bm);
/* Enable Compare channel A,B,C,D on timer C0 and D0 */TC0_EnableCCChannels( &TCD0, TC0_CCAEN_bm|TC0_CCBEN_bm| TC0_CCCEN_bm |
TC0_CCDEN_bm);TC0_EnableCCChannels( &TCC0, TC0_CCAEN_bm|TC0_CCBEN_bm| TC0_CCCEN_bm |
TC0_CCDEN_bm);
/* Start timer by selecting a clock source. */TC0_ConfigClockSource( &TCC0, TC_CLKSEL_DIV8_gc );
TC0_ConfigClockSource( &TCD0, TC_CLKSEL_DIV8_gc );
Page 11
TC1_ConfigClockSource( &TCC1, TC_CLKSEL_DIV8_gc ); TC1_ConfigClockSource( &TCD1, TC_CLKSEL_DIV8_gc );
/* Output new compare value. */ TC_SetCompareA( &TCC1, 255 ); TC_SetCompareB( &TCC1, 255 );
TC_SetCompareA( &TCD1, 255 );TC_SetCompareB( &TCD1, 255 );
TC_SetCompareA( &TCC0, 255 ); TC_SetCompareB( &TCC0, 255 ); TC_SetCompareC( &TCC0, 255 ); TC_SetCompareD( &TCC0, 255 );
TC_SetCompareA( &TCD0, 255 );TC_SetCompareB( &TCD0, 255 );TC_SetCompareC( &TCD0, 255 );TC_SetCompareD( &TCD0, 255 );
/* Wait for the new compare value to be latched * from CCABUF[H:L] to CCA[H:L]. This happens at * TC overflow (UPDATE ). */
while ( TC_GetOverflowFlag( &TCC0) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCC0);
while ( TC_GetOverflowFlag( &TCD0) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCD0);
while ( TC_GetOverflowFlag( &TCC1) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCC1);
while ( TC_GetOverflowFlag( &TCD1) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCD1);
}
/*move to position (from motion CSV file (for backward compatability)) */void moveMotor(int channel, int position, int speed)
Page 12
{//position = ((position + 255)/510) * divisions;
}
void newAxis(int speed, int positions[]){
for (int i =0; i < 10; i++){moveMotor(i, positions[i]);
}
}
int main(void){ //Config32MHzClock(); // configure sysclk=32MHz RC oscillator int i = 0; timer_init();
// configure PORTF:0 as output to LED PORTF.DIR=(1<<0); // Eval-01 64A3 RevA, Eval-USB while(1) {
//Blink LED so we know code isn't locked. PORTF.OUT ^= (1<<0); // Eval-01 64A3 RevA, Eval-USB
//_delay_ms(250);
while (i > LowerServoLimit){
/* Output new compare value. */TC_SetCompareA( &TCC1, i );TC_SetCompareB( &TCC1, i );
TC_SetCompareA( &TCD1, i );TC_SetCompareB( &TCD1, i );
TC_SetCompareA( &TCC0, i );TC_SetCompareB( &TCC0, i );TC_SetCompareC( &TCC0, i );TC_SetCompareD( &TCC0, i );
TC_SetCompareA( &TCD0, i );TC_SetCompareB( &TCD0, i );TC_SetCompareC( &TCD0, i );TC_SetCompareD( &TCD0, i );_delay_ms(tdelay);i--;
Page 13
}
while (i < UpperServoLimit){/* Output new compare value. */TC_SetCompareA( &TCC1, i );TC_SetCompareB( &TCC1, i );
TC_SetCompareA( &TCD1, i );TC_SetCompareB( &TCD1, i );
TC_SetCompareA( &TCC0, i );TC_SetCompareB( &TCC0, i );TC_SetCompareC( &TCC0, i );TC_SetCompareD( &TCC0, i );
TC_SetCompareA( &TCD0, i );TC_SetCompareB( &TCD0, i );TC_SetCompareC( &TCD0, i );TC_SetCompareD( &TCD0, i );_delay_ms(tdelay);i++;}
}
return 0;}
Better-walk.c (final example program)#define F_CPU 2000000UL //because i'm lazy...#define TCPeriod 1250 //Timer/Counter Period#define UpperServoLimit 575 //upper servo range limit with 1250 TC Period#define LowerServoLimit 175 //lower servo range limit with 1250 TC Period#define ServoPositions (UpperServoLimit - LowerServoLimit)#define tdelay 1000 //quick edit delay time
#define LOOPTIME 1
#define longestSpeedTime 9765; //in timer ticks
Page 14
#define TRUE 1#define FALSE 0
#include <stdio.h>#include <avr\io.h>#include <util\delay.h>#include "TC_driver.h"#include <string.h>
#define MAX_CHANNELS 10
void pwmTimerInit(){
/* Enable output on pins 0-5 */ PORTD.DIRSET = 63; PORTC.DIRSET = 63;
/* Set the TC period. */ TC_SetPeriod( &TCD0, TCPeriod );
TC_SetPeriod( &TCD1, TCPeriod ); TC_SetPeriod( &TCC0, TCPeriod );
TC_SetPeriod( &TCC1, TCPeriod );
/* Configure the TC for single slope mode. */TC0_ConfigWGM( &TCD0, TC_WGMODE_SS_gc );
TC0_ConfigWGM( &TCC0, TC_WGMODE_SS_gc ); TC1_ConfigWGM( &TCD1, TC_WGMODE_SS_gc ); TC1_ConfigWGM( &TCC1, TC_WGMODE_SS_gc );
/* Enable Compare channel A,B on timer C1 and D1 */ TC1_EnableCCChannels( &TCD1, TC1_CCAEN_bm|TC1_CCBEN_bm); TC1_EnableCCChannels( &TCC1, TC1_CCAEN_bm|TC1_CCBEN_bm);
/* Enable Compare channel A,B,C,D on timer C0 and D0 */TC0_EnableCCChannels( &TCD0, TC0_CCAEN_bm|TC0_CCBEN_bm| TC0_CCCEN_bm |
TC0_CCDEN_bm);TC0_EnableCCChannels( &TCC0, TC0_CCAEN_bm|TC0_CCBEN_bm| TC0_CCCEN_bm |
TC0_CCDEN_bm);
/* Start timer by selecting a clock source. */
Page 15
TC0_ConfigClockSource( &TCC0, TC_CLKSEL_DIV8_gc ); TC0_ConfigClockSource( &TCD0, TC_CLKSEL_DIV8_gc );
TC1_ConfigClockSource( &TCC1, TC_CLKSEL_DIV8_gc ); TC1_ConfigClockSource( &TCD1, TC_CLKSEL_DIV8_gc );
/* Output new compare value. */ TC_SetCompareA( &TCC1, 255 ); TC_SetCompareB( &TCC1, 255 );
TC_SetCompareA( &TCD1, 255 );TC_SetCompareB( &TCD1, 255 );
TC_SetCompareA( &TCC0, 255 ); TC_SetCompareB( &TCC0, 255 ); TC_SetCompareC( &TCC0, 255 ); TC_SetCompareD( &TCC0, 255 );
TC_SetCompareA( &TCD0, 255 );TC_SetCompareB( &TCD0, 255 );TC_SetCompareC( &TCD0, 255 );TC_SetCompareD( &TCD0, 255 );
/* Wait for the new compare value to be latched * from CCABUF[H:L] to CCA[H:L]. This happens at * TC overflow (UPDATE ). */
while ( TC_GetOverflowFlag( &TCC0) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCC0);
while ( TC_GetOverflowFlag( &TCD0) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCD0);
while ( TC_GetOverflowFlag( &TCC1) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCC1);
while ( TC_GetOverflowFlag( &TCD1) == 0 ) {};
/* Clear overflow flag. */ TC_ClearOverflowFlag( &TCD1);}
Page 16
void moveServo(uint8_t channel, int16_t position){
int16_t correctedPosition = (int16_t)(((int32_t)position + 255) * ServoPositions/512) + LowerServoLimit;
switch (channel){
case 0:TC_SetCompareA( &TCC0, correctedPosition );
break;case 1:
TC_SetCompareB( &TCC0, correctedPosition );break;case 2:
TC_SetCompareC( &TCC0, correctedPosition );break;case 3:
TC_SetCompareD( &TCC0, correctedPosition );break;case 4:
TC_SetCompareA( &TCC1, correctedPosition );break;case 5:
TC_SetCompareA( &TCD0, correctedPosition );break;case 6:
TC_SetCompareB( &TCD0, correctedPosition );break;case 7:
TC_SetCompareC( &TCD0, correctedPosition );break;case 8:
TC_SetCompareD( &TCD0, correctedPosition );break;case 9:
TC_SetCompareA( &TCD1, correctedPosition );break;
}}
/*parse position (taken from motion CSV file (for backward compatibility)) *///TODO: build headers and #include "" for importing motions
void robotPosition(int16_t positions[10], uint8_t speed){
//start the timer timing
Page 17
//speedTimerInit();
static int16_t oldPositions[10] = {0,0,0,0,0,0,0,0,0,0};
if (speed != 0){
int loops = 1;int notdone = TRUE;
// < (( (uint16_t)speed * longestSpeedTime) / 8)while ( notdone){
notdone = FALSE;
int16_t currentPosition;for (uint8_t i=0; i<MAX_CHANNELS-1; i++){
if (oldPositions[i] < positions[i]){currentPosition = oldPositions[i] + loops * (8-speed)/5;
if(currentPosition >= positions[i]) currentPosition = positions[i];else notdone = TRUE;
}
if (oldPositions[i] > positions[i]){currentPosition = oldPositions[i] - loops * (8-speed)/5;
if(currentPosition <= positions[i]) currentPosition = positions[i];else notdone = TRUE;
}
moveServo(i, currentPosition);}loops++;_delay_ms(LOOPTIME);
}
}
//set the final positionsfor (uint8_t j=0; j<MAX_CHANNELS; j++){
moveServo (j, positions[j]);}
//copy positions to old positions
Page 18
for (int k=0; k<MAX_CHANNELS; k++) oldPositions[k] = positions[k];
}
int main(void){
#define NUMOFCOMMANDS 35const int16_t command[NUMOFCOMMANDS][25] = {
{4,0,1,0,225,225,0,0,1,0,225,225,225,0,-35,-52,21,15,225,0,35,52,-21,15,225},{4,0,1,0,225,225,-30,0,1,0,225,225,225,-11,-35,-52,21,4,225,-12,35,53,-21,4,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-10,-75,-105,50,5,225,-9,75,60,-27,5,225},{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},{3,0,1,0,225,225,0,-25,1,0,225,225,225,-7,-60,-45,-5,8,225,-6,60,58,-28,8,225},{3,0,1,0,225,225,30,-25,1,0,225,225,225,4,-44,-60,20,19,225,4,44,49,-46,19,225},{2,0,1,0,225,225,30,0,1,0,225,225,225,5,-40,-60,27,20,225,7,40,105,-50,20,225},{3,17,1,0,225,225,30,0,1,0,225,225,225,6,-34,-58,28,21,225,7,34,45,5,21,225},{3,17,1,0,225,225,0,0,1,0,225,225,225,-4,-4,-49,46,11,225,-4,4,60,-20,11,225},{2,0,1,0,225,225,-30,0,1,0,225,225,225,-7,-75,-105,50,8,225,-5,75,60,-27,8,225},{3,0,1,0,225,225,0,0,1,0,225,225,225,-10,-45,-60,18,5,225,-9,45,53,-21,5,225},{5,0,1,0,225,225,0,0,1,0,225,225,225,0,-35,-52,21,15,225,0,35,52,-21,15,225}
};// // #define NUMOFCOMMANDS 2
Page 19
// const int16_t command[NUMOFCOMMANDS][25] = {// {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},//
{0,0,0,0,0,0,0,0,0,0,0,0,0,255,0,0,0,0,0,0,0,0,0,0,0},//
// };
pwmTimerInit();
// configure PORTF:0 as output to LED PORTF.DIR=(1<<0); // Eval-01 64A3 RevA, Eval-USB while(1) {
//Blink LED so we know code isn't locked. PORTF.OUT ^= (1<<0); // Eval-01 64A3 RevA, Eval-USB
//_delay_ms(250);
//int16_t maxPositions[10] = {255,255,255,255,255,255,255,255,255,255};//int16_t minPositions[10] = {-255,-255,-255,-255,-255,-255,-255,-255,-255,-255};
int16_t positions[10];
for (int i =0; i< NUMOFCOMMANDS; i++){
PORTF.OUT ^= (1<<0); // Eval-01 64A3 RevA, Eval-USB
int speed = command[i][0];positions[0] = command[i][13];positions[1] = command[i][14];positions[2] = command[i][15];positions[3] = command[i][16];positions[4] = command[i][17];positions[5] = command[i][19];positions[6] = command[i][20];positions[7] = command[i][21];positions[8] = command[i][22];positions[9] = command[i][23];
robotPosition(positions, speed);
Page 20
_delay_ms(1000);}
// _delay_ms(2000);// // moveServo(8, 100);// // _delay_ms(2000);// // moveServo(8, 0);//
};
return 0;};
KHR-1 Notes (initial development notes)PWM – 200Hz (5ms pulse period)Pulse Width - 700us-2.3msPower: 6VAmplitude: 1.5-5vOffset: 0.5(amplitude)
Calculations:(700 microseconds) / (5 milliseconds) = 0.14 or 14%
This gives (at 1250 divisions) 175 divisions(2.3 milliseconds) / (5 milliseconds) = 0.46 or 46%
This gives (at 1250 divisions) 575 divisions
Servo Movement:180 deg ~ 270 ms90 deg ~ 160 ms
Ports used
PC0 OC0APC1 OC0B
Page 21