Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- Servo servo_base, servo_joint;
- int servo_base_output = 120;
- int servo_joint_output = 90;
- int servo_pin_base = 10;
- int servo_pin_joint = 11;
- String stream, stream_base, stream_joint;
- char charBuf[50];
- char *i;
- char ascii_table[] = {' ', '!', '"', '#', '$', '%', '&', '\'', '(', ')', '*',
- '+', ',', '-', '.', '/', '0', '1', '2', '3', '4', '5',
- '6', '7', '8', '9', ':', ';', '<', '=', '>', '?', '@',
- 'A', 'B', 'C', 'D', 'E', 'F', 'G','H', 'I', 'J', 'K', 'L',
- 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
- 'Y', 'Z', '[', '\\', ']', '^', '_', '`', 'a', 'b', 'c', 'd',
- 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o',
- 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '{',
- '|', '}', '~'};
- char ascii_to_char(int ascii){
- return ascii_table[ascii - 32];
- }
- String capture_stream(){
- int input;
- String output;
- boolean flag = false;
- while((input = Serial.read()) != -1){
- output += String(ascii_to_char(input));
- flag = true;
- }
- if(flag){
- Serial.println(String(output));
- return output;
- }else{
- Serial.println(String("-1"));
- return "-1";
- }
- }
- void setup(){
- Serial.begin(9600);
- Serial.println("starting up");
- servo_base.attach(servo_pin_base);
- servo_joint.attach(servo_pin_joint);
- }
- void loop(){
- if((stream = capture_stream()) != "-1"){
- stream.toCharArray(charBuf, 50);
- stream_base = strtok_r(charBuf, ":", &i);
- stream_joint = strtok_r(NULL, ":", &i);
- if(stream_base == "-") stream_base = String(servo_base_output);
- if(stream_joint == "-") stream_joint = String(servo_joint_output);
- servo_base_output = stream_base.toInt();
- servo_joint_output = stream_joint.toInt();
- Serial.println("Base Servo: " + String(servo_base_output));
- Serial.println("Joint Servo: " + String(servo_joint_output));
- if(servo_base_output != -1){
- if(servo_base_output > 180){
- servo_base_output = 180;
- }else if(servo_base_output < 0){
- servo_base_output = 0;
- }
- servo_base.write(servo_base_output);
- }
- if(servo_joint_output != -1){
- if(servo_joint_output > 180){
- servo_joint_output = 180;
- }else if(servo_joint_output < 0){
- servo_joint_output = 0;
- }
- servo_joint.write(servo_joint_output);
- }
- }
- delay(500);
- }
Advertisement
Add Comment
Please, Sign In to add comment