7 years, 2 months ago.

Robotic snake servo motor keep spinning

Hi, Im still a student, so if i ask some stupid questions pls forgive me.

Im programming a robotic snake using LPC1768, the motors im using is Kondo krs 4034hv. This is my program... /media/uploads/JamesKaiLiang/untitled_document.pdf

When I compile my program, my motors keeps going round and round (not going to the positions i want). And sometimes only 1 of the motors is moving. Pls help me.

1 Answer

7 years, 2 months ago.

Hi James. Study lines 41 and 43 to see if you can spot an error. Your code (without your nice formatting):

#include "mbed.h"
#include <math.h>
#include "C12832.h"
#define pi 3.14159265358979323846264338327950288419716939937510582
#define amplitude 1000
C12832 lcd(p5, p7, p6, p8, p11);
Serial snake(p9, NC);
Serial pc(USBTX, USBRX);
DigitalOut myled1(LED1);
int i, j, y, m, n, counter=0; //m=motor id
float a, b, c, d, t;
unsigned short int motorInt[]={7600,7500,7000,7500,7400,7000,7200,7500,7300};
unsigned short int motorAngle[]={7600,7500,7000,7500,7400,7000,7200,7500,7300};
unsigned short int motorID[]={0,1,2,3,4,5,6,7,8};
unsigned short int motorPositions[]={7600,7500,7000,7500,7400,7000,7200,7500,7300};
unsigned short int shapeFactor[]={1,1.7,1.6,1.5,1.4,1.3,1.2,1.1,1};
Timer timer;
void motor_update(unsigned char Id, unsigned short int Position)
{
unsigned short int id,lo,hi;
snake.format(8, Serial::Even, 1); // setup for kondo motor
snake.baud(115200);
id=0x80|Id;
hi=(Position>>7)&0x007F;
lo=Position&0x007F;
snake.putc(id);
snake.putc(hi);
snake.putc(lo);
wait(0.001);
}
void position_update(void)
{
unsigned char motor;
for(motor=0;motor<sizeof(motorPositions);motor++)
{
motor_update(motorID[motor], motorPositions[motor]);
}
}
void SubStep(void)
{
for(j=0;j<10;j++)
{
motorAngle[m] = ((motorAngle[m]‐motorInt[m])/10)+motorInt[m];
}
}
int main()
{
myled1 = 0;
for(i=1;i<2;i++)
{
for (t=0;t<2;t+=0.025)
{
for (m=0;m<9;m++)
{
timer.start();
y = m‐1;
a = 1*pi; //a = angular frequency, b = phase shift
b = 2*pi/8;
c = a*t;
d = y*b;
motorAngle[m] = shapeFactor[m]*7500+(amplitude*sin(c+d));
SubStep();
motorPositions[m] = motorAngle[m];

position_update();
//wait_ms(25);
counter++;
}
myled1 = !myled1;
timer.read();
lcd.cls();
lcd.locate(0,3);
lcd.printf("time = %.4f sec.\ncount = %d",timer.read(),counter);
}
}
}

Thanks Sanjiv, I've made some minor changes to my codes, is this better?

/media/uploads/JamesKaiLiang/untitled_document_2.pdf

posted by James Neo 31 Jan 2017