![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
lighthouse
Dependencies: RobotArmController SerialHalfDuplex mbed
Fork of PR_RobotArm by
Diff: main.cpp
- Revision:
- 2:55f39e7883a6
- Parent:
- 0:ba8a9d66892d
diff -r 3ff94f82fddd -r 55f39e7883a6 main.cpp --- a/main.cpp Fri Feb 17 00:00:55 2017 +0000 +++ b/main.cpp Fri Jul 14 12:32:18 2017 +0000 @@ -1,41 +1,76 @@ #include "robotarm.h" +#include <math.h> +#include "serial.h" +#define ON 1 +#define OFF 0 Robotarm arm; +DigitalOut beacon(p15); +SerialControl serial; + int main() -{ - wait(1); // Useful if you need to connect H-Term etc. +{ + + int base_degree = 90; + int flag = 1; + //Run the main initialisation routine - arm.init(); + arm.init(); + serial.setup_serial_interfaces(); + /* Set all servos speed to 0.1 */ + servo.SetCRSpeed( BASE , 0.1 ); + servo.SetCRSpeed( SHOULDER , 0.1 ); + servo.SetCRSpeed( ELBOW , 0.1 ); + //Reset the servos to center position (after 1 second delay) //NB This activates the servos (makes rigid) so be careful when using arm.zero_servos(1); //Wait till servos are zeroed wait(3); - //Initialise remote control - if(REMOTE_ENABLED == 1)remote.init(); - - //User code can now go in a loop: - while(1) { - //Eg set all servos to 1948 then 2148 - servo.SetGoal(BASE,1948,1); - servo.SetGoal(SHOULDER,1948,1); - servo.SetGoal(ELBOW,1948,1); - servo.SetGoal(WRIST,400,1); - //If you want to show detailed info about a servo over serial, use the following: - //servo.DebugData(WRIST); - wait(0.5); - servo.SetGoal(BASE,2148,1); - servo.SetGoal(SHOULDER,2148,1); - servo.SetGoal(ELBOW,2148,1); - servo.SetGoal(WRIST,600,1); - wait(0.5); - //Alternatively we can set all the servos then use trigger - observe the difference... - //servo.SetGoal(BASE,2148,0); - //servo.SetGoal(SHOULDER,2148,0); - //servo.SetGoal(ELBOW,2148,0); - //servo.SetGoal(WRIST,600,0); - //servo.trigger(); - //wait(0.5); + display.clear_display(); + display.set_position(0,0); + display.write_string("start"); + servo.SetGoalDegrees( BASE , 0 , 1); + servo.SetGoalDegrees( SHOULDER , 0 , 1 ); + servo.SetGoalDegrees( ELBOW , 90 , 1 ); + while(1){ + if(turning == 1){ + beacon = ON; + if(flag == 1){ + if(base_degree >= 180){ + flag = 0; + } + else { + base_degree += 5; + } + } + else if(flag == 0){ + if(base_degree <= 0){ + flag = 1; + } + else { + base_degree -= 5; + } + } + servo.SetGoalDegrees( BASE , base_degree-90 , 1); + } + else if(turning == 0){ + // turning = 3; + beacon = ON; + servo.SetGoalDegrees( BASE , base_degree - 90 , 1); + pc.printf("base_degree = %d \n", base_degree - 90); + } + else if(turning == 2){ + beacon = OFF; + servo.SetGoalDegrees( BASE , 0 , 1); + base_degree = 90; + wait(0.5); + } } -} \ No newline at end of file + +} + + + +