Dyanette Arroyo / Mbed 2 deprecated MBEDROBOTARM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:dcdfdfd1f10c, 2022-11-17 (annotated)

Committer:
darro011
Date:
Thu Nov 17 21:19:48 2022 +0000
Revision:
0:dcdfdfd1f10c
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darro011 0:dcdfdfd1f10c 1 #include "mbed.h"
darro011 0:dcdfdfd1f10c 2
darro011 0:dcdfdfd1f10c 3 Serial async_port(p9, p10);
darro011 0:dcdfdfd1f10c 4
darro011 0:dcdfdfd1f10c 5 char command [10]; // command 3 aarray
darro011 0:dcdfdfd1f10c 6 char command_stop [4]; // command 6 array
darro011 0:dcdfdfd1f10c 7
darro011 0:dcdfdfd1f10c 8 // constant header value
darro011 0:dcdfdfd1f10c 9 #define SERVO_FRAME_HEADER 0x55
darro011 0:dcdfdfd1f10c 10
darro011 0:dcdfdfd1f10c 11 // Servo ID
darro011 0:dcdfdfd1f10c 12 #define ID_1 0x01
darro011 0:dcdfdfd1f10c 13 #define ID_2 0x02
darro011 0:dcdfdfd1f10c 14 #define ID_3 0x03
darro011 0:dcdfdfd1f10c 15 #define ID_4 0x04
darro011 0:dcdfdfd1f10c 16 #define ID_5 0x05
darro011 0:dcdfdfd1f10c 17 #define ID_6 0x06
darro011 0:dcdfdfd1f10c 18
darro011 0:dcdfdfd1f10c 19 #define GET_LOW_BYTE(A) (uint8_t)((A))
darro011 0:dcdfdfd1f10c 20 //Macro function get lower 8 bits of A
darro011 0:dcdfdfd1f10c 21 #define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
darro011 0:dcdfdfd1f10c 22 //Macro function get higher 8 bits of A
darro011 0:dcdfdfd1f10c 23
darro011 0:dcdfdfd1f10c 24 void moveOneServo(char ID,int16_t position){
darro011 0:dcdfdfd1f10c 25 // move any servo at a time, parameters (ID_# , position 0-1000)
darro011 0:dcdfdfd1f10c 26
darro011 0:dcdfdfd1f10c 27 if(position < 0)
darro011 0:dcdfdfd1f10c 28 position = 0;
darro011 0:dcdfdfd1f10c 29 if(position > 1000)
darro011 0:dcdfdfd1f10c 30 position = 1000;
darro011 0:dcdfdfd1f10c 31
darro011 0:dcdfdfd1f10c 32 command [0] = command [1] = SERVO_FRAME_HEADER;
darro011 0:dcdfdfd1f10c 33 command [2] = 0x08; // length
darro011 0:dcdfdfd1f10c 34 command [3] = 0x03; // Command
darro011 0:dcdfdfd1f10c 35 command [4] = 0x01; // # of servos
darro011 0:dcdfdfd1f10c 36 command [5] = 0xD8; // Ltime
darro011 0:dcdfdfd1f10c 37 command [6] = 0x07; // Htime
darro011 0:dcdfdfd1f10c 38 command [7] = ID; // ID
darro011 0:dcdfdfd1f10c 39 command [8] = GET_LOW_BYTE(position);
darro011 0:dcdfdfd1f10c 40 command [9] = GET_HIGH_BYTE(position);
darro011 0:dcdfdfd1f10c 41
darro011 0:dcdfdfd1f10c 42 for(int i=0; i<10; i++){async_port.putc(command[i]);}
darro011 0:dcdfdfd1f10c 43
darro011 0:dcdfdfd1f10c 44 /*/ notes:
darro011 0:dcdfdfd1f10c 45 CMD_SERVO_MOVE ( command 3)
darro011 0:dcdfdfd1f10c 46 */
darro011 0:dcdfdfd1f10c 47 }
darro011 0:dcdfdfd1f10c 48
darro011 0:dcdfdfd1f10c 49 int main() {
darro011 0:dcdfdfd1f10c 50
darro011 0:dcdfdfd1f10c 51 // note: wait should be 5s to allow the servo enough time to position itself
darro011 0:dcdfdfd1f10c 52
darro011 0:dcdfdfd1f10c 53 async_port.baud(9600); // boud rate provided by user manual
darro011 0:dcdfdfd1f10c 54
darro011 0:dcdfdfd1f10c 55 while(1) {
darro011 0:dcdfdfd1f10c 56
darro011 0:dcdfdfd1f10c 57 moveOneServo(ID_3,0);
darro011 0:dcdfdfd1f10c 58 wait(5);
darro011 0:dcdfdfd1f10c 59 moveOneServo(ID_3,1000);
darro011 0:dcdfdfd1f10c 60 wait(5);
darro011 0:dcdfdfd1f10c 61
darro011 0:dcdfdfd1f10c 62 //Testing
darro011 0:dcdfdfd1f10c 63 ////////////////////////
darro011 0:dcdfdfd1f10c 64 /*
darro011 0:dcdfdfd1f10c 65 closeGrabber();
darro011 0:dcdfdfd1f10c 66 wait(1);
darro011 0:dcdfdfd1f10c 67 //stopProgram();
darro011 0:dcdfdfd1f10c 68 //wait(1);
darro011 0:dcdfdfd1f10c 69 openGrabber();
darro011 0:dcdfdfd1f10c 70 //wait(1);
darro011 0:dcdfdfd1f10c 71 //stopProgram();
darro011 0:dcdfdfd1f10c 72 wait(1);
darro011 0:dcdfdfd1f10c 73 ////////////////////////
darro011 0:dcdfdfd1f10c 74 for(int i=0; i<10; i++){async_port.putc(close_grabber[i]);}
darro011 0:dcdfdfd1f10c 75 wait(1);
darro011 0:dcdfdfd1f10c 76 for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
darro011 0:dcdfdfd1f10c 77 wait(1);
darro011 0:dcdfdfd1f10c 78 for(int i=0; i<10; i++){async_port.putc(open_grabber[i]);}
darro011 0:dcdfdfd1f10c 79 wait(1);
darro011 0:dcdfdfd1f10c 80 for(int i=0; i<10; i++){async_port.putc(stop_prog[i]);}
darro011 0:dcdfdfd1f10c 81 wait(1);
darro011 0:dcdfdfd1f10c 82 */
darro011 0:dcdfdfd1f10c 83 }
darro011 0:dcdfdfd1f10c 84
darro011 0:dcdfdfd1f10c 85 }
darro011 0:dcdfdfd1f10c 86 void closeGrabber(){ // function to only close grabber
darro011 0:dcdfdfd1f10c 87
darro011 0:dcdfdfd1f10c 88 command [0] = command [1] = SERVO_FRAME_HEADER;
darro011 0:dcdfdfd1f10c 89 command [2] = 0x08;
darro011 0:dcdfdfd1f10c 90 command [3] = 0x03;
darro011 0:dcdfdfd1f10c 91 command [4] = 0x01;
darro011 0:dcdfdfd1f10c 92 command [5] = 0xE8;
darro011 0:dcdfdfd1f10c 93 command [6] = 0x03;
darro011 0:dcdfdfd1f10c 94 command [7] = 0x01;
darro011 0:dcdfdfd1f10c 95 command [8] = 0xD8;
darro011 0:dcdfdfd1f10c 96 command [9] = 0x07;
darro011 0:dcdfdfd1f10c 97
darro011 0:dcdfdfd1f10c 98 for(int i=0; i<10; i++){async_port.putc(command[i]);}
darro011 0:dcdfdfd1f10c 99
darro011 0:dcdfdfd1f10c 100 /*/ notes:
darro011 0:dcdfdfd1f10c 101 CMD_SERVO_MOVE ( command 3)
darro011 0:dcdfdfd1f10c 102 */
darro011 0:dcdfdfd1f10c 103 }
darro011 0:dcdfdfd1f10c 104 void openGrabber(){// function to only open grabber
darro011 0:dcdfdfd1f10c 105
darro011 0:dcdfdfd1f10c 106 command [0] = command [1] = SERVO_FRAME_HEADER;
darro011 0:dcdfdfd1f10c 107 command [2] = 0x08;
darro011 0:dcdfdfd1f10c 108 command [3] = 0x03;
darro011 0:dcdfdfd1f10c 109 command [4] = 0x01;
darro011 0:dcdfdfd1f10c 110 command [5] = 0xE8;
darro011 0:dcdfdfd1f10c 111 command [6] = 0x03;
darro011 0:dcdfdfd1f10c 112 command [7] = 0x01;
darro011 0:dcdfdfd1f10c 113 command [8] = 0x00;
darro011 0:dcdfdfd1f10c 114 command [9] = 0x00;
darro011 0:dcdfdfd1f10c 115
darro011 0:dcdfdfd1f10c 116 for(int i=0; i<10; i++){async_port.putc(command[i]);}
darro011 0:dcdfdfd1f10c 117 /*/ notes:
darro011 0:dcdfdfd1f10c 118 CMD_SERVO_MOVE ( command 3)
darro011 0:dcdfdfd1f10c 119 */
darro011 0:dcdfdfd1f10c 120 }
darro011 0:dcdfdfd1f10c 121 void stopProgram(){// function to stop all programs
darro011 0:dcdfdfd1f10c 122
darro011 0:dcdfdfd1f10c 123 command [0] = command [1] = SERVO_FRAME_HEADER;
darro011 0:dcdfdfd1f10c 124 command [2] = 0x02;
darro011 0:dcdfdfd1f10c 125 command [3] = 0x07;
darro011 0:dcdfdfd1f10c 126
darro011 0:dcdfdfd1f10c 127 for(int i=0; i<10; i++){async_port.putc(command[i]);}
darro011 0:dcdfdfd1f10c 128
darro011 0:dcdfdfd1f10c 129 /*/ notes:
darro011 0:dcdfdfd1f10c 130 CMD_ACTION_STOP( command 6)
darro011 0:dcdfdfd1f10c 131 */
darro011 0:dcdfdfd1f10c 132
darro011 0:dcdfdfd1f10c 133 }