Merge pull request #59 from lintangsutawika/milestone1

Pushed milestone1 legacy programs

robot/main/master.cc.Milestone_1

@@ -0,0 +1,162 @@
+#include <arduino/Arduino.h>
+#include <mavlink/v1.0/common/mavlink.h>
+#include <arduino/Wire.h>
+
+#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;
+}

robot/main/slave-serve.cc.Milestone_1

@@ -0,0 +1,127 @@
+//Slave-Serve
+#include <arduino/Arduino.h>
+#include <DynamixelSerial/DynamixelSerial.h>
+#include <arduino/Wire.h>
+
+
+#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;
+    }
+  }
+}