void balancing(){ //thread balancing function
while(1){
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle += imu.getRoll();
usleep(50);
angle =angle/4*180/3.1415;
pid.calculate_speed(angle,silniki.getLeftSpeed(),silniki.getRightSpeed(),steering,throttle,finalleftspeed,finalrightspeed);
if(balancingactive)silniki.setSpeed(finalleftspeed,finalrightspeed,0.0,4);
//printf("%f\n",angle);
if(angle>40.0){
actual = Position::layfront;
balancingactive = 0;
}
else if (angle<-40.0){
actual = Position::layback;
balancingactive = 0;
}
else actual = Position::standing;
angle = 0.0;
}
}
int main()
{
Network network;
Accelerometer imu;
imu.bypassDrift();
motors.setToZero();
signal(SIGABRT, sigHandler);
signal(SIGINT, sigHandler);
signal(SIGKILL, sigHandler);
signal(SIGQUIT, sigHandler);
signal(SIGTERM, sigHandler);
float ypr[3];
while(true) {
if(imu.getFIFOCount() > 42) {
imu.getYawPitchRoll(ypr);
float p_computed = p_pid.compute(ypr[1], p_target), r_computed = r_pid.compute(ypr[2], r_target);
motors.setSpeed(MOTOR_FL, throttle + r_computed - p_computed);
motors.setSpeed(MOTOR_BL, throttle + r_computed + p_computed);
motors.setSpeed(MOTOR_FR, throttle - r_computed - p_computed);
motors.setSpeed(MOTOR_BR, throttle - r_computed + p_computed);
network.send(SET_MEASURED_VALUES, ypr, sizeof(float)*3, false);
}
}
exit(EXIT_SUCCESS);
}
// the loop routine runs over and over again forever:
void loop()
{
wprintf(L"Calibration in 3 seconds\n");
delay(3000);
wprintf(L"Calibrating...");
gyroscope.calibrate();
wprintf(L"Cycle tyme in microseconds: %ld \n", TICK_LENGTH_MICROSECONDS);
// main cycle
delay(100);
unsigned long lastIteration = micros();
unsigned long nextIteration = lastIteration + TICK_LENGTH_MICROSECONDS;
int n = 0;
do {
float actualAngle, deltaAngle;
while (micros() < nextIteration)
{
// do nothing
}
unsigned long now = micros();
gyroscope.getAngleFiltered(actualAngle, deltaAngle, now);
targetAngle = angleRegulator.getResult(speedIntegrator.getSum(), 0, now - lastIteration);
float motorSpeed = 0;
motorSpeed = speedRegulator.getResult(actualAngle - targetAngle, deltaAngle, now - lastIteration);
n++;
if (n % 20 == 0) // Print on every 20th iteration
{
wprintf(L"%d\t%f\t%f\t%f\t%f\n", n, targetAngle, actualAngle, deltaAngle, motorSpeed);
}
motors.setSpeedBoth(motorSpeed);
speedIntegrator.pushValue(motorSpeed);
lastIteration = now;
nextIteration = now + TICK_LENGTH_MICROSECONDS;
keyboard.processEvents();
if (keyboard.shouldExit())
{
break;
}
} while (true);
motors.stopAll();
wprintf(L"Exiting...\n");
exit(0);
}
void cv_keyboard(unsigned char key, int x, int y)
{
static Motors m = Motors(&model);
if (key == 'q' || key == 27) { // q or escape
destroy ();
exit(0);
}
else if (key == 'p') {
//screenshot();
cvSaveImage ("cvSimImgFwd.jpg", cv_img_fwd);
cvSaveImage ("cvSimImgDwn.jpg", cv_img_down);
}
else { // pass the key to the motor object to handle
m.key_command(key);
}
glutPostRedisplay();
}
int main(int argc, char **argv)
{
signal(SIGINT, stop);
ConfigFile config("walk.yml");
config.useCommandArgs(argc, argv);
walk.loadConfig(config);
config.help();
try {
Robots robots;
cout << "Starting " << endl;
robots.loadYaml("config.yml");
Robot *robot = robots["local"];
walk.setRobot(robot);
Motors *motors = robot->getMotors();
Sensors *sensors = robot->getSensors();
cout << "Starting motors " << endl;
motors->start(100);
sensors->start(100);
ms_sleep(100);
while (true) {
walk.tick(0.01);
ms_sleep(10);
}
ms_sleep(25);
cout << "Bye bye" << endl;
robots.stop();
} catch(string exc) {
cout << "Received exception " << exc << endl;
exit(0);
}
}
void setup()
{
motors.init();
gyroscope.init();
keyboard.init();
/*
PID coefficients may vary depending on mechanical properties of the robot/motors/battery
Here are some instructions how to tune the PID
http://robotics.stackexchange.com/questions/167/what-are-good-strategies-for-tuning-pid-loops
*/
// Angle regulator. This PID adjusts the desired angle (normally 0) depending
// on the mean speed of the robot. The goal is to keep the mean speed near 0.
// (robot is staying on one place and does not drive away)
angleRegulator.init(-0.02, 0, 0); // P is negative, because to compensate positive speed we mast tilt the robot little bit back
// Speed regulator. This PID gets the desired angle from the angle regulator and
// adjusts the acceleration (power applied to motors) to keep the angle near the desired value.
speedRegulator.init(35, 0.2, 0);
wprintf(L"Press Esc to exit\n");
}
int main(void)
{
silniki.disable();
imu.setup(); // initialize IMU
usleep(1000);
silniki.enable();
//if(!lipol.isGood())printf("niski poziom napiecia baterii");
imu.resetFIFO();
pid.timerStart(); //start pid timer
std::thread balance(balancing); //create balancing thread
usleep(500000);
usleep(500000);
while(1){
switch (actual){
case standing:
balancingactive =1;
usleep(200000);
break;
case layfront:
std::cout<<"I'm trying to stand front"<<std::endl;
silniki.setSpeed(0.0,0.0,0.0,4);
silniki.disable();
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
silniki.enable();
silniki.setSpeed(400.0,400.0,0.0,1);
usleep(200000);
silniki.setSpeed(500.0,500.0,0.0,1);
usleep(200000);
silniki.setSpeed(550.0,550.0,0.0,1);
usleep(500000);
silniki.setSpeed(-400.0,-400.0,0.0,1);
usleep(150000);
silniki.setSpeed(-500.0,-500.0,0.0,1);
usleep(50000);
actual = Position::standing;
std::cout<<"I'm standing"<<std::endl;
break;
case layback:
std::cout<<"I'm trying to stand back"<<std::endl;
silniki.setSpeed(0.0,0.0,0.0,1);
silniki.disable();
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
usleep(500000);
silniki.enable();
silniki.setSpeed(-400.0,-400.0,0.0,1);
usleep(200000);
silniki.setSpeed(-500.0,-500.0,0.0,1);
usleep(200000);
silniki.setSpeed(-550.0,-550.0,0.0,1);
usleep(500000);
silniki.setSpeed(350.0,350.0,0.0,1);
usleep(100000);
silniki.setSpeed(550.0,550.0,0.0,1);
usleep(50000);
actual = Position::standing;
std::cout<<"I'm standing"<<std::endl;
break;
}
}
balance.detach();
return 0;
}
int main(int argc, char **argv)
{
try
{
Robots robots;
cout << "Starting " << endl;
// Charge la configuration et connecte les robots
robots.loadYaml("config.yml");
Robot * robot = robots["bras"];
Motors * motors = robot->getMotors();
Moves * moves = robot->getMoves();
cout << "Starting motors " << endl;
motors->start(30);
syst_wait_ms(1000);
cout << "Putting all motors compliant " << endl;
robot->allCompliant();
syst_wait_ms(1000);
cout << "Starting record move..." << endl;
moves->startMove("Recorder",0,0);
syst_wait_ms(500);
cout << "Recording for ten seconds..." << endl;
moves->startRecordingSpline();
syst_wait_ms(10000);
cout << "... done." << endl;
moves->stopRecordingSpline();
moves->stopMove("Recorder",0);
cout << "Retrieving recorded spline" << endl;
LinearSpline spline = moves->getSpline();
cout << "Spline has " << spline.sequences.size() << "sequencess " << spline.sequences[0].points.size() << "points " << endl;
cout << "Sending back slower spline" << endl;
spline.speed_factor *= 0.5;
moves->setSpline(spline);
/* cout << "Taking initial position..." << endl;
motors->goToInit();
cout << "...done." << endl;
*/
cout << "Playing slower spline..." << endl;
moves->playSpline();
syst_wait_ms(2000);
cout << "...done." << endl;
moves->stopSpline();
cout << "Putting all motors compliant " << endl;
robot->allCompliant();
syst_wait_ms(1000);
cout << "Bye bye" << endl;
robots.stop();
}
catch(string exc)
{
cout << "Received exception " << exc << endl;
exit(0);
}
}
void sigHandler(int sig)
{
std::cout << "[ERROR] Received signal " << sig << std::endl;
motors.setToZero();
exit(sig);
}
void loop()
{
g_elapser.process();
g_blinker.process(g_elapser.elapse());
g_motors.process(g_elapser.elapse());
}
// main function
int main(void) {
t_cpu_time timeout;
t_cpu_time timeout_mpu;
float deltat_2;
float roll_first, pitch_first;
board_init();
hal.init(0,NULL);
hal.analogin->init();
sysclk_init();
init_sys_clocks();
float initials[3];
int16_t magnetic[3];
float yaw_first;
float soft[3],hard[3];
float desti[2];
float kp = 4.8;
float kpp = 0.7;
float kd = 0;
mpu9150.initMPU9150(FCPU_HZ);
mpu9150.initHMC58();
mpu9150.calibrate_mpu9150(initials);
int16_t myMagData[3];
hal.uartB->println("NEE ZAFERINDEN BAHSEDIYORSUUN");
float percent = 40;
uint8_t c, last_c;
uint16_t count = 0;
cpu_set_timeout(cpu_ms_2_cy(10, FOSC0), &timeout_mpu);
cpu_set_timeout(cpu_ms_2_cy(1000, FOSC0), &timeout);
hal.uartB->println("VER COSKUYU");
c = hal.uartB->read();
motor.motors_init();
while(1){
if (usart_test_hit(&AVR32_USART4)) {
hal.uartB->println("I am hit");
last_c = c;
c = hal.uartB->read();
if(c == '9') {
motor.kill_motors();
hal.uartB->println("KIRDIN BENI GOD DAMNIT");
while(1);
}
if (c == '8') {
percent += 1;
hal.uartB->print("Percent Increased to: ");
hal.uartB->println(percent);
}
if (c == '2') {
percent -= 1;
hal.uartB->print("Percent Decreased to: ");
hal.uartB->println(percent);
}
if (c == 'u') {
kpp = kpp + 0.1;
hal.uartB->print("Kpp is: ");
hal.uartB->println(kpp);
}
if (c == 'j') {
kpp = kpp - 0.1;
hal.uartB->print("Kpp is: ");
hal.uartB->println(kpp);
}
if (c == 't') {
kd = kd + 0.001;
hal.uartB->print("Kd is: ");
hal.uartB->println(kd);
}
if (c == 'g') {
kd = kd - 0.001;
hal.uartB->print("Kd is: ");
hal.uartB->println(kd);
}
if (c == 'y') {
kp = kp + 0.1;
hal.uartB->print("Kp is: ");
hal.uartB->println(kp);
}
if (c == 'h') {
kp = kp - 0.1;
hal.uartB->print("Kp is: ");
hal.uartB->println(kp);
}
c = last_c;
}
if (c == '5') {
//
if(cpu_is_timeout(&timeout_mpu)) {
cpu_stop_timeout(&timeout_mpu);
mpu9150.update();
PID_Pitch = Pitch_Controller(pitch,initials[1],kp,kpp,0,kd,0.01f);
motor.motor1_update(percent,PID_Pitch,0,0);
motor.motor3_update(percent,PID_Pitch,0,0);
cpu_set_timeout(cpu_ms_2_cy(10, FOSC0), &timeout_mpu);
}
// if(cpu_is_timeout(&timeout)) {
// cpu_stop_timeout(&timeout);
// hal.uartB->println(PID_Pitch);
//
// cpu_set_timeout(cpu_ms_2_cy(1000, FOSC0), &timeout);
// }
// float PID_Pitch = Pitch_Controller(pitch,initials[1],kp,kd,0,0,deltat);
// motor.motor1_update(percent,PID_Pitch,0,0);
// deltat =(float) cpu_cy_2_ms((Get_sys_count()),FOSC0)/1000;
// cpu_set_timeout( cpu_ms_2_cy(10000, FOSC0), &timeout);
// Set_sys_count(0);
}
}
}