Yujia Gao / Mbed 2 deprecated lighthouse_2

lighthouse

Dependencies:   RobotArmController SerialHalfDuplex mbed

Fork of PR_RobotArm by James Hilder

main.cpp@2:55f39e7883a6, 2017-07-14 (annotated)

Committer:
JessicaGao
Date:
Fri Jul 14 12:32:18 2017 +0000
Revision:
2:55f39e7883a6
Parent:
0:ba8a9d66892d 
lighthouse;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:ba8a9d66892d 1 #include "robotarm.h"
JessicaGao 2:55f39e7883a6 2 #include <math.h>
JessicaGao 2:55f39e7883a6 3 #include "serial.h"
JessicaGao 2:55f39e7883a6 4 #define ON 1
JessicaGao 2:55f39e7883a6 5 #define OFF 0
jah128 0:ba8a9d66892d 6
jah128 0:ba8a9d66892d 7 Robotarm arm;
JessicaGao 2:55f39e7883a6 8 DigitalOut beacon(p15);
JessicaGao 2:55f39e7883a6 9 SerialControl serial;
JessicaGao 2:55f39e7883a6 10
jah128 0:ba8a9d66892d 11
jah128 0:ba8a9d66892d 12 int main()
JessicaGao 2:55f39e7883a6 13 {
JessicaGao 2:55f39e7883a6 14
JessicaGao 2:55f39e7883a6 15 int base_degree = 90;
JessicaGao 2:55f39e7883a6 16 int flag = 1;
JessicaGao 2:55f39e7883a6 17
jah128 0:ba8a9d66892d 18 //Run the main initialisation routine
JessicaGao 2:55f39e7883a6 19 arm.init();
JessicaGao 2:55f39e7883a6 20 serial.setup_serial_interfaces();
JessicaGao 2:55f39e7883a6 21 /* Set all servos speed to 0.1 */
JessicaGao 2:55f39e7883a6 22 servo.SetCRSpeed( BASE , 0.1 );
JessicaGao 2:55f39e7883a6 23 servo.SetCRSpeed( SHOULDER , 0.1 );
JessicaGao 2:55f39e7883a6 24 servo.SetCRSpeed( ELBOW , 0.1 );
JessicaGao 2:55f39e7883a6 25
jah128 0:ba8a9d66892d 26 //Reset the servos to center position (after 1 second delay)
jah128 0:ba8a9d66892d 27 //NB This activates the servos (makes rigid) so be careful when using
jah128 0:ba8a9d66892d 28 arm.zero_servos(1);
jah128 0:ba8a9d66892d 29 //Wait till servos are zeroed
jah128 0:ba8a9d66892d 30 wait(3);
JessicaGao 2:55f39e7883a6 31 display.clear_display();
JessicaGao 2:55f39e7883a6 32 display.set_position(0,0);
JessicaGao 2:55f39e7883a6 33 display.write_string("start");
JessicaGao 2:55f39e7883a6 34 servo.SetGoalDegrees( BASE , 0 , 1);
JessicaGao 2:55f39e7883a6 35 servo.SetGoalDegrees( SHOULDER , 0 , 1 );
JessicaGao 2:55f39e7883a6 36 servo.SetGoalDegrees( ELBOW , 90 , 1 );
JessicaGao 2:55f39e7883a6 37 while(1){
JessicaGao 2:55f39e7883a6 38 if(turning == 1){
JessicaGao 2:55f39e7883a6 39 beacon = ON;
JessicaGao 2:55f39e7883a6 40 if(flag == 1){
JessicaGao 2:55f39e7883a6 41 if(base_degree >= 180){
JessicaGao 2:55f39e7883a6 42 flag = 0;
JessicaGao 2:55f39e7883a6 43 }
JessicaGao 2:55f39e7883a6 44 else {
JessicaGao 2:55f39e7883a6 45 base_degree += 5;
JessicaGao 2:55f39e7883a6 46 }
JessicaGao 2:55f39e7883a6 47 }
JessicaGao 2:55f39e7883a6 48 else if(flag == 0){
JessicaGao 2:55f39e7883a6 49 if(base_degree <= 0){
JessicaGao 2:55f39e7883a6 50 flag = 1;
JessicaGao 2:55f39e7883a6 51 }
JessicaGao 2:55f39e7883a6 52 else {
JessicaGao 2:55f39e7883a6 53 base_degree -= 5;
JessicaGao 2:55f39e7883a6 54 }
JessicaGao 2:55f39e7883a6 55 }
JessicaGao 2:55f39e7883a6 56 servo.SetGoalDegrees( BASE , base_degree-90 , 1);
JessicaGao 2:55f39e7883a6 57 }
JessicaGao 2:55f39e7883a6 58 else if(turning == 0){
JessicaGao 2:55f39e7883a6 59 // turning = 3;
JessicaGao 2:55f39e7883a6 60 beacon = ON;
JessicaGao 2:55f39e7883a6 61 servo.SetGoalDegrees( BASE , base_degree - 90 , 1);
JessicaGao 2:55f39e7883a6 62 pc.printf("base_degree = %d \n", base_degree - 90);
JessicaGao 2:55f39e7883a6 63 }
JessicaGao 2:55f39e7883a6 64 else if(turning == 2){
JessicaGao 2:55f39e7883a6 65 beacon = OFF;
JessicaGao 2:55f39e7883a6 66 servo.SetGoalDegrees( BASE , 0 , 1);
JessicaGao 2:55f39e7883a6 67 base_degree = 90;
JessicaGao 2:55f39e7883a6 68 wait(0.5);
JessicaGao 2:55f39e7883a6 69 }
jah128 0:ba8a9d66892d 70 }
JessicaGao 2:55f39e7883a6 71
JessicaGao 2:55f39e7883a6 72 }
JessicaGao 2:55f39e7883a6 73
JessicaGao 2:55f39e7883a6 74
JessicaGao 2:55f39e7883a6 75
JessicaGao 2:55f39e7883a6 76