void OnTimer(int value)
{
glutTimerFunc(20, &OnTimer, value);
old_t = t;
t = (GLfloat)glutGet(GLUT_ELAPSED_TIME)/1000.0;
delta_t = (t - old_t);
//printf("%f\n", delta_t);
GLfloat tmpppp[3] = {cow.pos.x, cow.pos.y, cow.pos.z};
glUniform3fv(glGetUniformLocation(g_shader, "cow_pos"), 1, tmpppp);
turn_cow(&cow, -m_angle);
update_cow(&cow, delta_t);
update_fence(&ff, &cow, delta_t);
move_cow(&cow, m_angle);
update_floor(&f, &cow);
update_wall(&wall, &cow, delta_t);
update_ball(&ball, &cow, delta_t);
update_farmer(&farmer);
if(keyIsDown('k'))
update_ragdoll(&ragdoll, delta_t);
if(check_collision_2(&cow.bb, &wall.bb))
{
//printf("yey\n");
glUniform1i(glGetUniformLocation(g_shader, "collision"), 1);
//cow.momentum = SetVector(-cow.momentum.x, cow.momentum.y, -cow.momentum.z);
//wall_collision(wall, cow);
}
else
glUniform1i(glGetUniformLocation(g_shader, "collision"), 0);
//printf("%f, %f\n", delta_t, old_t);
glUniform1f(glGetUniformLocation(g_shader, "time"), t);
mat4 proj_matrix = frustum(-1, 1, -1, 1, 1, 750.0);
mat4 cam_matrix = lookAt(cam_dist*cos(m_angle)+cow.pos.x, 9+cow.pos.y, cam_dist*sin(m_angle)+cow.pos.z, cow.pos.x, 8.5+cow.pos.y, cow.pos.z, 0.0, 1.0, 0.0);
//mat4 cam_matrix = lookAt(200+200*cos(cam_angle), 0, 200, 0, 8, 0, 0.0, 1.0, 0.0);
//g_shader = loadShaders("shader.vert" , "shader.frag");
//glUseProgram(g_shader);
glUniformMatrix4fv(glGetUniformLocation(g_shader, "proj_matrix"), 1, GL_TRUE, proj_matrix.m);
glUniformMatrix4fv(glGetUniformLocation(g_shader, "cam_matrix"), 1, GL_TRUE, cam_matrix.m);
//glutDisplayFunc(DisplayWindow);
//draw_cow(&cow, g_shader);
//draw_bone(&bone, g_shader);
mat4 tmp, testjoint, testjoint2;
testjoint = IdentityMatrix();
joint_s * j = &head_joint[0];
joint_s * jc = j->child[0];
joint_s * jcc = jc->child[0];
//joint_s * jp = j->parent;
//joint_s * jpp = jp->parent;
float Ms[8][4*4];
float legMs[8][4*4];
float currpos[8*3] = {0};
float bonepos[8*3] = {0};
float legcurrpos[8*3] = {0};
float legbonepos[8*3] = {0};
//GLfloat * Mtmp = Ms;
int i=0, ii=0;
//jc->R = ArbRotate(SetVector(0,0,1), cos(4*t/(i+1))/2.5);
//j->R = ArbRotate(SetVector(0,0,1), 0);
j->R = ArbRotate(SetVector(0,0,1), sin(4*t/(3+1))/1.2);
//jcc->R = ArbRotate(SetVector(0,0,1), cos(8*t/(2+1))/4);
mat4 Mpacc, Minvacc, tmptrans, tmppp, invtrans;
tmppp = IdentityMatrix();
float freq = 7;//Norm(cow.speed)/4;
if(Norm(cow.momentum) < .5)
freq = 0;
else if(Norm(cow.momentum) > 5)
freq = 20;
//printf("%f\n", freq);
j = &thigh_joint[0];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/1.5);
jc->R = ArbRotate(SetVector(0,0,1), cos(freq*t)/2.5);
j = &thigh_joint[1];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), -cos(freq*t)/2.5);
jc->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/2.5);
j = &thigh_joint[2];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/1.5);
jc->R = ArbRotate(SetVector(0,0,1), cos(freq*t)/2.5);
j = &thigh_joint[3];
jc = j->child[0];
j->R = ArbRotate(SetVector(0,0,1), -cos(freq*t)/2.5);
jc->R = ArbRotate(SetVector(0,0,1), sin(freq*t)/2.5);
j = &legbase_joint[0];
//j->R = ArbRotate(SetVector(1,0,0), -cos(7*t/(i+1)));
//j->R = ArbRotate(SetVector(1,0,0), -M_PI/6);
j = &thigh_joint[0];
//j->R = Mult(ArbRotate(SetVector(1,0,0), cos(7*t/(i+1))), j->R);
//j->R = Mult(ArbRotate(SetVector(1,0,0), M_PI/6), j->R);
i = 0;
tmppp = IdentityMatrix();
//legbase_joint[0].R = ArbRotate(SetVector(0,0,1), cos(3*t)/2);
//legs
calc_bone_transform(&legbase_joint[0], 0);
calc_bone_transform(&legbase_joint[1], 0);
calc_bone_transform(&legbase_joint[2], 0);
calc_bone_transform(&legbase_joint[3], 0);
j = &tail_joint[0];
j->R = ArbRotate(SetVector(0,0,1), -M_PI/2.5);
j = &tail_joint[1];
j->R = ArbRotate(SetVector(0,0,1), -M_PI/8.5);
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), 0);
j = &tail_joint[2];
//j->R = ArbRotate(SetVector(0,0,1), 0);
j = &tail_joint[3];
j->R = ArbRotate(SetVector(0,0,1), 0);
//body
j = &body_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), cos(t*7)/9.5);
j->R = ArbRotate(SetVector(0,1,0), m_angle + cow.angle);
j->R = Mult(j->R, ArbRotate(SetVector(0,0,1), cos(freq*t)/9));
calc_bone_transform(&body_joint[0],0);
//tail
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), -M_PI/2.5);
//j = &tail_joint[1];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//j = &tail_joint[2];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//j = &tail_joint[3];
//j->R = ArbRotate(SetVector(0,0,1), 0);
//calc_bone_transform(&tail_joint[0],0);
//head
j = &head_joint[0];
j->R = ArbRotate(SetVector(0,0,1), sin(7*t)/9.5);
calc_bone_transform(&head_joint[0],0);
j = &farmer.skeleton.joints[2];
//j->R = Rx(cos(4*t));
j->R = Mult(Rx(M_PI/2.2 + sin(5*t)/11), Ry(cos(-5*t)/2));
j = &farmer.skeleton.joints[3];
//j->R = Rx(cos(4*t));
j->R = Mult(Rx(-M_PI/2.2 + sin(5*t)/11), Ry(cos(5*t)/2));
//left shoulder (her right)
//jc = &farmer.skeleton.joints[3];
//jc->R = Rx(-sin(3*t));
jc = &farmer.skeleton.joints[4];
jc->R = Ry(M_PI/3 + cos(5*t)/8);
jc = &farmer.skeleton.joints[5];
jc->R = Ry(-M_PI/3 + cos(5*t)/8);
jc = &farmer.skeleton.joints[10];
//jc->R = Rz(M_PI/3);
jc->R = Rz(.5-cos(5*t));
jc = &farmer.skeleton.joints[12];
jc->R = Rz((-1-cos(5*t))/2);
jc = &farmer.skeleton.joints[11];
//jc->R = Rz(M_PI/3);
jc->R = Rz(.5+cos(5*t));
jc = &farmer.skeleton.joints[13];
jc->R = Rz((-1-cos(5*t))/2);
calc_bone_transform(&farmer.skeleton.joints[0], 0);
calc_bone_transform(&farmer.skeleton.joints[2], 0);
calc_bone_transform(&farmer.skeleton.joints[3], 0);
//calc_bone_transform(&farmer.skeleton.joints[4], 0);
calc_bone_transform(&farmer.skeleton.joints[1], 0);
calc_bone_transform(&farmer.skeleton.joints[10], 0);
calc_bone_transform(&farmer.skeleton.joints[11], 0);
// calc_bone_transform(&farmer.skeleton.joints[8], 0);
// calc_bone_transform(&farmer.skeleton.joints[9], 0);
glutPostRedisplay();
}
/*
* Controller functionality
* - Send elevator location messages to callboxes
* - Listen for button press messages
*/
void controller() {
byte sw1_pressed = 0, sw2_pressed = 0;
byte rxmessage[PAYLOAD_SIZE]; // Received data payload
byte button_pressed;
byte next_floor;
char *button_floor_str, *button_direction_str;
byte update_lcd = 1;
byte cycle_count = 0;
word distance; // car height in cm, distance measurement in mm
byte cur_floor;
byte last_floor = 0;
//byte b; // used for debug manual frame sending testing
dist_init();
mctrl_init(); // Initialize servo motor controller
for(;;) {
mctrl_update();
cycle_count++;
if ( cycle_count == 10 ) {
update_lcd = 1;
cycle_count = 0;
}
if ( update_lcd ) {
update_lcd = 0;
/*
#ifdef USE_LCD
if ( cur_floor == 0 ) {
LCDclear();
LCDputs("No car");
} else {
LCDclear();
LCDprintf("%dmm/F%d", car_height, cur_floor);
}
#endif
*/
}
// CAN bus <-> serial link
// Check for new incoming messages and send out received messages via serial
runSerialCAN(MSCAN_NODE_ID);
/*
while ( sci_bytesAvailable() ) {
sci_readByte(&b);
lcd_putc(b);
} */
if(data_available()) {
CANget(rxmessage);
switch(rxmessage[0]) {
case CMD_BUTTON_CALL:
button_pressed = rxmessage[1];
addToQueue(button_pressed);
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
switch(cur_floor) {
case FLOOR1:
button_floor_str = "1";
break;
case FLOOR2:
button_floor_str = "2";
break;
case FLOOR3:
button_floor_str = "3";
break;
default:
break;
}
if(next_floor == cur_floor) {
button_direction_str = "stat";
} else if(next_floor > cur_floor) {
button_direction_str = "up ";
} else {
button_direction_str = "down";
}
#ifdef USE_LCD
LCDhome();
LCDprintf("\nFloor%s Dir %s", button_floor_str, button_direction_str);
#else
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Floor");
lcd_puts(button_floor_str);
lcd_puts(" Dir ");
lcd_puts(button_direction_str);
#endif
#endif
break;
case CMD_BUTTON_CAR:
button_pressed = rxmessage[1];
if(button_pressed == BUTTON_STOP) {
// call emergency stop function
} else if(button_pressed < BUTTON_DOOR_CLOSE) {
addToQueue(button_pressed);
}
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
switch(cur_floor) {
case FLOOR1:
button_floor_str = "1";
break;
case FLOOR2:
button_floor_str = "2";
break;
case FLOOR3:
button_floor_str = "3";
break;
default:
break;
}
if(next_floor == cur_floor){
button_direction_str = "stat";
}else if(next_floor > cur_floor){
button_direction_str = "up ";
}else {
button_direction_str = "down";
}
#ifdef USE_LCD
LCDhome();
LCDprintf("\nFloor%s Dir %s", button_floor_str, button_direction_str);
#endif
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Floor");
lcd_puts(button_floor_str);
lcd_puts(" Dir ");
lcd_puts(button_direction_str);
#endif
break;
case CMD_DISTANCE:
distance = (rxmessage[1] << 8) | rxmessage[2];
pid_feedback(distance);
if (distance < SETPOINT_F1 + FLOOR_MARGIN) cur_floor = FLOOR1;
if (distance > SETPOINT_F2 - FLOOR_MARGIN && distance < SETPOINT_F2 + FLOOR_MARGIN) cur_floor = FLOOR2;
if (distance > SETPOINT_F3 - FLOOR_MARGIN && distance < SETPOINT_F3 + FLOOR_MARGIN) cur_floor = FLOOR3;
if ( distance > 1500 ) {
cur_floor = 0;
#ifdef USE_LED7
led7_write(led7_bars[1]);
#endif
} else {
#ifdef USE_LED7
led7_write(led7_table[cur_floor]);
#endif
if ( cur_floor != last_floor ) {
update_floor(cur_floor);
last_floor = cur_floor;
// if we have reached the target floor, pop off the top of the queue
// TODO: change name of getNextFloor() to be more descriptive
if(cur_floor == next_floor){
getNextFloor();
next_floor = peekNextFloor();
pid_setpoint(FLOOR2SETPOINT(next_floor));
}
}
}
#ifdef USE_LCD
LCDhome();
LCDprintf("Dist: %4d", distance);
#endif
break;
case CMD_ERROR:
break;
default:
#ifdef USE_LCD
LCDclear();
LCDputs("\nUnknown command");
#endif
#ifdef USE_LCD2
lcd_goto(0x10); // Start at second line
lcd_puts("Unknown command");
#endif
break;
}
}
delay_ms(100);
}
}
