From 4cde63498d2d41a4a846a1a4297319c05cc17d01 Mon Sep 17 00:00:00 2001 From: Lintang Sutawika Date: Sat, 22 Nov 2014 00:53:02 +0700 Subject: [PATCH] Pushed milestone1 legacy programs --- robot/main/master.cc.Milestone_1 | 162 ++++++++++++++++++++++++++ robot/main/slave-serve.cc.Milestone_1 | 127 ++++++++++++++++++++ robot/main/slave-wheel.cc.Milestone_1 | 130 +++++++++++++++++++++ 3 files changed, 419 insertions(+) create mode 100644 robot/main/master.cc.Milestone_1 create mode 100644 robot/main/slave-serve.cc.Milestone_1 create mode 100644 robot/main/slave-wheel.cc.Milestone_1 diff --git a/robot/main/master.cc.Milestone_1 b/robot/main/master.cc.Milestone_1 new file mode 100644 index 0000000..67ade3d --- /dev/null +++ b/robot/main/master.cc.Milestone_1 @@ -0,0 +1,162 @@ +#include +#include +#include + +#define SLAVE_ON +#define DEBUG_LEVEL_1 + +int main() { + init();// this needs to be called before setup() otherwise some functions won't work + Serial.begin(57600); + #ifdef SLAVE_ON + Serial1.begin(9600); + Serial2.begin(9600); + Serial3.begin(9600); + pinMode(13,OUTPUT); + pinMode(4,OUTPUT); + digitalWrite(4,LOW); + + const uint8_t header= 0xCE; + const uint8_t footer= 0xEE; + + const uint8_t hit_mode= 0x0A; + const uint8_t position_mode= 0x0B; + const uint8_t hit_flag= 0x0F; + + float slave1_cmd_speed_f = 0.0; + float slave2_cmd_speed_f = 0.0; + float slave3_cmd_speed_f = 0.0; + #endif + + const uint16_t rate = 50; + while (true) { + const uint8_t desired_msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; + + mavlink_message_t rx_msg; + mavlink_status_t rx_status; + bool found = false; + + //Serial.println("Waiting ..."); + while (!found) { + if (Serial.available() > 0) { + if (mavlink_parse_char(MAVLINK_COMM_0, Serial.read(), &rx_msg, &rx_status)) { + if ( (rx_msg.msgid==desired_msgid) ) { + found = true; + } + } + } + } + + // Dispatch + switch (rx_msg.msgid) { + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { + // + mavlink_set_position_target_local_ned_t msg; + mavlink_msg_set_position_target_local_ned_decode(&rx_msg, &msg); + + #ifdef DEBUG_LEVEL_1 + analogWrite(13,msg.vx); + // Serial.println(msg.vx); + // Serial.print("msg.vx=speed_wheel_1= "); Serial.println(msg.vx); + // Serial.print("msg.vy=speed_wheel_2= "); Serial.println(msg.vy); + // Serial.print("msg.vz=speed_wheel_3= "); Serial.println(msg.vz); + //Serial.print("msg.yaw=hit_service= "); Serial.println(msg.yaw); + #endif + + #ifdef SLAVE_ON + // 2. break down the speed into the speed for 3 motors + slave1_cmd_speed_f = msg.vx; + slave2_cmd_speed_f = msg.vy; + slave3_cmd_speed_f = msg.vz; + // Interpret the service command + float hit_service = msg.yaw; + + + + int slave1_cmd_speed= abs(slave1_cmd_speed_f); + int slave2_cmd_speed= abs(slave2_cmd_speed_f); + int slave3_cmd_speed= abs(slave3_cmd_speed_f); + + //3. dispatch and send the 3 speed values + uint8_t buffer1[7]; + uint8_t buffer2[7]; + uint8_t buffer3[7]; + + buffer1[0]= header; + buffer1[6]= footer; + buffer1[2]= (slave1_cmd_speed >> 8) & 0xFF; + buffer1[1]= slave1_cmd_speed & 0xFF; + if(slave1_cmd_speed_f < 0.0f) buffer1[3]= 0x0C; + else if (slave1_cmd_speed_f >= 0.0f) buffer1[3]= 0xCC; + uint16_t checksum1= buffer1[0] + buffer1[1] + buffer1[2] + buffer1[6]; + buffer1[4]= checksum1 & 0x00FF; + buffer1[5]= (checksum1 & 0xFF00) >> 8; + + buffer2[0]= header; + buffer2[6]= footer; + buffer2[2]= (slave2_cmd_speed >> 8) & 0xFF; + buffer2[1]= slave2_cmd_speed & 0xFF; + if(slave2_cmd_speed_f < 0.0f) buffer2[3]= 0x0C; + else if (slave2_cmd_speed_f >= 0.0f) buffer2[3]= 0xCC; + uint16_t checksum2= buffer2[0] + buffer2[1] + buffer2[2] + buffer2[6]; + buffer2[4]= checksum2 & 0x00FF; + buffer2[5]= (checksum2 & 0xFF00) >> 8; + + buffer3[0]= header; + buffer3[6]= footer; + buffer3[2]= (slave3_cmd_speed >> 8) & 0xFF; + buffer3[1]= slave3_cmd_speed & 0xFF; + if(slave3_cmd_speed_f < 0.0f) buffer3[3]= 0x0C; + else if (slave3_cmd_speed_f >= 0.0f) buffer3[3]= 0xCC; + uint16_t checksum3= buffer3[0] + buffer3[1] + buffer3[2] + buffer3[6]; + buffer3[4]= checksum3 & 0x00FF; + buffer3[5]= (checksum3 & 0xFF00) >> 8; + + Serial1.write(buffer1, 7); + Serial2.write(buffer2, 7); + Serial3.write(buffer3, 7); + + if (hit_service > 0.0) { + //hit-manipulation slave + int8_t slave_hit1_speed; + int8_t slave_hit2_speed; + int target_pos1; + int target_pos2; + uint8_t buffer_hit1[11]; + uint8_t buffer_hit2[11]; + uint8_t buffer_hit3[11]; + + // TODO: Send hit_service command! + buffer_hit3[0]= header; + buffer_hit3[10]= footer; + buffer_hit3[1]= hit_mode; + buffer_hit3[2]= hit_flag; + buffer_hit3[6]= (int) ((slave_hit1_speed & 0xFF000000) >> 24); + buffer_hit3[5]= (int) ((slave_hit1_speed & 0x00FF0000) >> 16); + buffer_hit3[4]= (int) ((slave_hit1_speed & 0x0000FF00) >> 8); + buffer_hit3[3]= (int) ((slave_hit1_speed & 0x000000FF)); + buffer_hit3[7]= target_pos1; + uint16_t checksum_hit1= buffer_hit1[0] + buffer_hit1[1] + buffer_hit1[2] + buffer_hit1[3] + buffer_hit1[4] + buffer_hit1[5] + buffer_hit1[6] + buffer_hit1[7] + buffer_hit1[10]; + buffer_hit3[8]= (int) ((checksum_hit1 & 0x00FF)); + buffer_hit3[9]= (int) ((checksum_hit1 & 0xFF00)); + + analogWrite(13,255); + Wire.beginTransmission(93); + Wire.write(buffer_hit3, 11); + Wire.endTransmission(); + hit_service = 0.0; + digitalWrite(4,HIGH); + analogWrite(13,0); + delay(100); + digitalWrite(4,LOW); + } + + #endif + }// case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED + }// switch (rx_msg.msgid) + + delay(1000/rate);; + }// while (true) + + return 0; +} diff --git a/robot/main/slave-serve.cc.Milestone_1 b/robot/main/slave-serve.cc.Milestone_1 new file mode 100644 index 0000000..788c528 --- /dev/null +++ b/robot/main/slave-serve.cc.Milestone_1 @@ -0,0 +1,127 @@ +//Slave-Serve +#include +#include +#include + + +#define ARM_ID 15 +#define GRIP_ID 14 +#define DYNA_CON 2 +#define PNEU_PIN 3 + +//Constants +#define RETRACT 0 +#define EXTEND 1 +#define SER_DELAY 210 //ms + +int val=0; +uint8_t buffer[11]; +int received_data; + +// function that executes whenever data is received from master +// this function is registered as an event, see setup() +void receiveEvent(int howMany) +{ + int i = 0; + while(0 < Wire.available()) // loop through all + { + buffer[i] = Wire.read(); // receive byte as a character + i++; + } + + int checksum= (buffer[8] << 8); + checksum |= buffer[7]; + if(checksum == (buffer[0] + buffer[1] + buffer[2] + buffer[3] + buffer[4] + buffer[9])) //Serial.println("OK"); + //else Serial.println("ERROR"); +} + +//Positioning arm to service +void armDeploy(){ + int angle = 150-120; + int angle_tr = (double)angle * 1023 / 300; + Dynamixel.move(ARM_ID,angle_tr); +} + +//Positioning arm to initialize hitter +void armRetract(){ + int angle = 150+120; + int angle_tr = (double)angle * 1023 / 300; + Dynamixel.move(ARM_ID,angle_tr); +} + +//Dropping shuttlecock +void gripperOpen(){ + int angle = 150 + 60; + int angle_tr = (double)angle * 1023 / 300; + //Dynamixel.move(GRIP_ID,angle_tr); + Dynamixel.move(GRIP_ID,0); +} + +//Close the gripper +void gripperClose(){ + int angle = 150; + int angle_tr = (double)angle * 1023 / 300; + //Dynamixel.move(GRIP_ID,angle_tr); + Dynamixel.move(GRIP_ID,512); + +} + +//Position the racket to bottom (ready for shooting) +void racketBottom(){ + digitalWrite(PNEU_PIN,RETRACT); +} + +//Shot the racket up! +void racketUp(){ + digitalWrite(PNEU_PIN,EXTEND); +} + +//Service routine +void serviceShot(){ + //armDeploy(); + armRetract(); + delay(2000); + + gripperOpen(); + delay(SER_DELAY); + + racketUp(); + delay(1000); + + racketBottom(); + gripperClose(); + armDeploy(); + //armRetract(); +} + +//initialization needed +void serviceInit(){ + Dynamixel.begin(1000000,2); + Dynamixel.setEndless(ARM_ID,OFF); + Dynamixel.setEndless(GRIP_ID,OFF); + Dynamixel.setMaxTorque(ARM_ID,512); + Dynamixel.setMaxTorque(GRIP_ID,512); + Dynamixel.move(ARM_ID,240*1023/300); + Dynamixel.move(GRIP_ID,150*1023/300); + delay(1000); + pinMode(PNEU_PIN,OUTPUT); + digitalWrite(PNEU_PIN,HIGH); + +} + +int main(){ + init(); + // put your setup code here, to run once: + serviceInit(); + Wire.begin(93); // join i2c bus with address #93 + Wire.onReceive(receiveEvent); // register event + + while(1){ + if(buffer[2] == 0x0F){ + //SERVE!!!!! + serviceShot(); + delay(5000); + buffer[2] == 0x00; + } + } +} diff --git a/robot/main/slave-wheel.cc.Milestone_1 b/robot/main/slave-wheel.cc.Milestone_1 new file mode 100644 index 0000000..3b7b0bb --- /dev/null +++ b/robot/main/slave-wheel.cc.Milestone_1 @@ -0,0 +1,130 @@ +#include +#include +#include +#include + +int timer1_counter; +const size_t pwm_pin = 11; +const size_t dir_pin = 12; +const float outmax = 100.0; +const float outmin = -100.0; + +const int encoder_out_a_pin = 2; +const int encoder_out_b_pin = 3; +const int encoder_resolution = 360; //Only needed for Initialization, not used unless .rot() is called + +int speed; +int bufferSpeed; + +trui::Motor motor(pwm_pin, dir_pin, encoder_out_a_pin, encoder_out_b_pin, encoder_resolution, outmax, outmin); + +void setup() +{ + + // initialize timer1 + noInterrupts(); // disable all interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + + // set compare match register to desired timer count: + OCR1A = 780; + // turn on CTC mode: + TCCR1B |= (1 << WGM12); + // Set CS10 and CS12 bits for 1024 prescaler: + TCCR1B |= (1 << CS10); + TCCR1B |= (1 << CS12); + // enable timer compare interrupt: + TIMSK1 |= (1 << OCIE1A); + interrupts(); // enable all interrupts + speed = 0; +} + +ISR(TIMER1_COMPA_vect) // interrupt service routine +{ + //interrupts(); + motor.set_speed(speed); +} + +int main() { + init();// this needs to be called before setup() or some functions won't work there + setup(); + pinMode(13,OUTPUT); + motor.setup(); + + Serial.begin(9600); + + long timeNow = 0, timeOld = 0; + int incoming_byte = 0; + byte buffer[7] = {0,0,0,0,0,0,0}; + int flag=0; + while (true) { + + + // //1. Wait msg from master + if (Serial.available() >= 7) { + buffer[0] = Serial.read(); + buffer[1] = Serial.read(); + buffer[2] = Serial.read(); + buffer[3] = Serial.read(); + buffer[4] = Serial.read(); + buffer[5] = Serial.read(); + buffer[6] = Serial.read(); + } + // if(Serial.available()){ + // //Serial.findUntil + // if(Serial.read() == 206 ){ + + // buffer[1] = Serial.read(); + // buffer[2] = Serial.read(); + // buffer[3] = Serial.read(); + // buffer[4] = Serial.read(); + // buffer[5] = Serial.read(); + // buffer[6] = Serial.read(); + // } + // } + + //noInterrupts(); + // bufferS + //bufferSpeed = (buffer[2] << 8); + //bufferSpeed |= (buffer[1]); + //interrupts(); + + speed = buffer[1];//((buffer[2]<<8) | (buffer[1]));//bufferSpeed; + + //if(bufferSpeed <= 500 && bufferSpeed >=-500) speed = bufferSpeed; + //else speed = 0; + //speed = 50;//(int)bufferSpeed; + if(buffer[3] == 0x0C) speed= -1*speed; + else if(buffer[3] == 0xCC) speed= speed; + // speed = 50; + //speed = 5; //Default speed + //Serial.println(speed); + delay(1); + // //2. Identify msg + // //2A. If msgid = manual_setpoint, set speed control to cmd_speed + // if ( (rx_msg.msgid==set_speed_msgid) ) { + // mavlink_manual_setpoint_t msg; + // mavlink_msg_manual_setpoint_decode(&rx_msg, &msg); + + // speed= msg.roll; + // } + // //2B. If msgid = asking for actual speed, send back actual speed to master + // else if ( (rx_msg.msgid==actual_speed_query_msgid) ) { + // float actual_speed; + // mavlink_message_t msg_to_master; + // uint8_t system_id= MAV_TYPE_RBMT; + // uint8_t component_id= MAV_COMP_ID_ARDUINO_SLAVE1; + + // uint32_t time_boot_ms= millis(); + // mavlink_msg_manual_setpoint_pack(system_id, component_id, &msg_to_master, time_boot_ms, actual_speed, 0., 0., 0., 0, 0); + + // uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + // uint16_t len = mavlink_msg_to_send_buffer(buf, &msg_to_master); + + // Serial.write(buf, len); + // } + + } + + return 0; +}