For Cansat mission mbed
Dependencies: MU2Class RS405cb mbed myCAN_logger
main.cpp@0:7857b4c95c75, 2013-07-13 (annotated)
- Committer:
- YSB
- Date:
- Sat Jul 13 17:17:40 2013 +0000
- Revision:
- 0:7857b4c95c75
- Child:
- 1:f19466574b75
First Publish
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YSB | 0:7857b4c95c75 | 1 | #include "mbed.h" |
YSB | 0:7857b4c95c75 | 2 | #include "RS405cb.h" |
YSB | 0:7857b4c95c75 | 3 | #include "MU2Class.h" |
YSB | 0:7857b4c95c75 | 4 | |
YSB | 0:7857b4c95c75 | 5 | //for debug |
YSB | 0:7857b4c95c75 | 6 | Serial pc(USBTX,USBRX); |
YSB | 0:7857b4c95c75 | 7 | DigitalOut led2(LED2); |
YSB | 0:7857b4c95c75 | 8 | //for communication |
YSB | 0:7857b4c95c75 | 9 | CAN can(p30, p29); |
YSB | 0:7857b4c95c75 | 10 | MU2Class MyMu2(p9,p10); |
YSB | 0:7857b4c95c75 | 11 | DigitalOut MU2_permit(p11); |
YSB | 0:7857b4c95c75 | 12 | //for drive actuator |
YSB | 0:7857b4c95c75 | 13 | //PwmOut motor1(p21); |
YSB | 0:7857b4c95c75 | 14 | RS405cb servo(p13,p14,p18);//TX,RX,PERMIT |
YSB | 0:7857b4c95c75 | 15 | //for fusing PWM |
YSB | 0:7857b4c95c75 | 16 | PwmOut nikuromu1(p22); |
YSB | 0:7857b4c95c75 | 17 | PwmOut nikuromu2(p23); |
YSB | 0:7857b4c95c75 | 18 | PwmOut nikuromu3(p24); |
YSB | 0:7857b4c95c75 | 19 | //for Flight confirmation |
YSB | 0:7857b4c95c75 | 20 | DigitalIn flightpin(p26); |
YSB | 0:7857b4c95c75 | 21 | |
YSB | 0:7857b4c95c75 | 22 | //for CAN |
YSB | 0:7857b4c95c75 | 23 | #define CAN_BAUD 4000000 |
YSB | 0:7857b4c95c75 | 24 | int id; |
YSB | 0:7857b4c95c75 | 25 | char smsg[8]; |
YSB | 0:7857b4c95c75 | 26 | |
YSB | 0:7857b4c95c75 | 27 | //for PWM |
YSB | 0:7857b4c95c75 | 28 | #define M_PERIOD 0.01 |
YSB | 0:7857b4c95c75 | 29 | #define MOTOR_OFF motor1.pulsewidth(0.0); |
YSB | 0:7857b4c95c75 | 30 | #define N_PERIOD 0.01 |
YSB | 0:7857b4c95c75 | 31 | #define NIKUROMU1_OFF nikuromu1.pulsewidth(0.0); |
YSB | 0:7857b4c95c75 | 32 | #define NIKUROMU2_OFF nikuromu2.pulsewidth(0.0); |
YSB | 0:7857b4c95c75 | 33 | #define NIKUROMU3_OFF nikuromu3.pulsewidth(0.0); |
YSB | 0:7857b4c95c75 | 34 | #define NIKUROMU1_ON nikuromu1.pulsewidth(0.003); |
YSB | 0:7857b4c95c75 | 35 | #define NIKUROMU2_ON nikuromu2.pulsewidth(0.003); |
YSB | 0:7857b4c95c75 | 36 | #define NIKUROMU3_ON nikuromu3.pulsewidth(0.003); |
YSB | 0:7857b4c95c75 | 37 | |
YSB | 0:7857b4c95c75 | 38 | //for servo |
YSB | 0:7857b4c95c75 | 39 | void RHI_Scan(void); |
YSB | 0:7857b4c95c75 | 40 | |
YSB | 0:7857b4c95c75 | 41 | int main() { |
YSB | 0:7857b4c95c75 | 42 | //correspondence off |
YSB | 0:7857b4c95c75 | 43 | MU2_permit = 1;//off |
YSB | 0:7857b4c95c75 | 44 | |
YSB | 0:7857b4c95c75 | 45 | //PWM init |
YSB | 0:7857b4c95c75 | 46 | //motor1.period(M_PERIOD); |
YSB | 0:7857b4c95c75 | 47 | //MOTOR_OFF |
YSB | 0:7857b4c95c75 | 48 | nikuromu1.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 49 | NIKUROMU1_OFF |
YSB | 0:7857b4c95c75 | 50 | nikuromu2.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 51 | NIKUROMU2_OFF |
YSB | 0:7857b4c95c75 | 52 | nikuromu3.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 53 | NIKUROMU3_OFF |
YSB | 0:7857b4c95c75 | 54 | |
YSB | 0:7857b4c95c75 | 55 | //for CAN init |
YSB | 0:7857b4c95c75 | 56 | CANMessage msg; |
YSB | 0:7857b4c95c75 | 57 | can.frequency(CAN_BAUD); |
YSB | 0:7857b4c95c75 | 58 | |
YSB | 0:7857b4c95c75 | 59 | |
YSB | 0:7857b4c95c75 | 60 | //for servo test |
YSB | 0:7857b4c95c75 | 61 | //led2 =1; |
YSB | 0:7857b4c95c75 | 62 | //RHI_Scan(); |
YSB | 0:7857b4c95c75 | 63 | //servo.Rotate_Servo_Float_Test(3); |
YSB | 0:7857b4c95c75 | 64 | //servo.ID_CHANGE(1,3); |
YSB | 0:7857b4c95c75 | 65 | /* |
YSB | 0:7857b4c95c75 | 66 | float theta; |
YSB | 0:7857b4c95c75 | 67 | theta=-150.0; |
YSB | 0:7857b4c95c75 | 68 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 69 | servo.Rotate_Servo_Float(3,theta); |
YSB | 0:7857b4c95c75 | 70 | while(1) { |
YSB | 0:7857b4c95c75 | 71 | if(can.read(msg)) { |
YSB | 0:7857b4c95c75 | 72 | pc.printf("Message received: %d,%d\n\r", msg.data[0],msg.data[2]); |
YSB | 0:7857b4c95c75 | 73 | if(msg.data[2]!=1){ |
YSB | 0:7857b4c95c75 | 74 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 75 | theta += 1.0; |
YSB | 0:7857b4c95c75 | 76 | servo.Rotate_Servo_Float(3,theta); |
YSB | 0:7857b4c95c75 | 77 | //wait(0.02); |
YSB | 0:7857b4c95c75 | 78 | } |
YSB | 0:7857b4c95c75 | 79 | else{servo.TORQUE_ON(3);} |
YSB | 0:7857b4c95c75 | 80 | led2 = !led2; |
YSB | 0:7857b4c95c75 | 81 | } |
YSB | 0:7857b4c95c75 | 82 | } |
YSB | 0:7857b4c95c75 | 83 | */ |
YSB | 0:7857b4c95c75 | 84 | |
YSB | 0:7857b4c95c75 | 85 | //Sequence |
YSB | 0:7857b4c95c75 | 86 | |
YSB | 0:7857b4c95c75 | 87 | while(flightpin ==1){} |
YSB | 0:7857b4c95c75 | 88 | |
YSB | 0:7857b4c95c75 | 89 | MU2_permit = 0;//on |
YSB | 0:7857b4c95c75 | 90 | |
YSB | 0:7857b4c95c75 | 91 | wait(5); |
YSB | 0:7857b4c95c75 | 92 | |
YSB | 0:7857b4c95c75 | 93 | |
YSB | 0:7857b4c95c75 | 94 | id=1; |
YSB | 0:7857b4c95c75 | 95 | smsg[0]=1; |
YSB | 0:7857b4c95c75 | 96 | can.write(CANMessage(id,smsg)); |
YSB | 0:7857b4c95c75 | 97 | |
YSB | 0:7857b4c95c75 | 98 | NIKUROMU1_ON |
YSB | 0:7857b4c95c75 | 99 | wait(6.0); |
YSB | 0:7857b4c95c75 | 100 | NIKUROMU1_OFF |
YSB | 0:7857b4c95c75 | 101 | |
YSB | 0:7857b4c95c75 | 102 | wait(10); |
YSB | 0:7857b4c95c75 | 103 | |
YSB | 0:7857b4c95c75 | 104 | float theta; |
YSB | 0:7857b4c95c75 | 105 | theta=-150.0; |
YSB | 0:7857b4c95c75 | 106 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 107 | servo.Rotate_Servo_Float(3,theta); |
YSB | 0:7857b4c95c75 | 108 | while(1) { |
YSB | 0:7857b4c95c75 | 109 | if(can.read(msg)) { |
YSB | 0:7857b4c95c75 | 110 | pc.printf("Message received:%d,%d,%d\n\r",msg.id, msg.data[0],msg.data[2]); |
YSB | 0:7857b4c95c75 | 111 | if(msg.data[2]!=1){ |
YSB | 0:7857b4c95c75 | 112 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 113 | theta += 1.0; |
YSB | 0:7857b4c95c75 | 114 | servo.Rotate_Servo_Float(3,theta); |
YSB | 0:7857b4c95c75 | 115 | } |
YSB | 0:7857b4c95c75 | 116 | else{ |
YSB | 0:7857b4c95c75 | 117 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 118 | break; |
YSB | 0:7857b4c95c75 | 119 | } |
YSB | 0:7857b4c95c75 | 120 | led2 = !led2; |
YSB | 0:7857b4c95c75 | 121 | } |
YSB | 0:7857b4c95c75 | 122 | } |
YSB | 0:7857b4c95c75 | 123 | |
YSB | 0:7857b4c95c75 | 124 | |
YSB | 0:7857b4c95c75 | 125 | wait(5.0); |
YSB | 0:7857b4c95c75 | 126 | NIKUROMU2_ON |
YSB | 0:7857b4c95c75 | 127 | wait(6.0); |
YSB | 0:7857b4c95c75 | 128 | NIKUROMU2_OFF |
YSB | 0:7857b4c95c75 | 129 | |
YSB | 0:7857b4c95c75 | 130 | wait(5.0); |
YSB | 0:7857b4c95c75 | 131 | NIKUROMU3_ON |
YSB | 0:7857b4c95c75 | 132 | wait(6.0); |
YSB | 0:7857b4c95c75 | 133 | NIKUROMU3_OFF |
YSB | 0:7857b4c95c75 | 134 | |
YSB | 0:7857b4c95c75 | 135 | RHI_Scan(); |
YSB | 0:7857b4c95c75 | 136 | |
YSB | 0:7857b4c95c75 | 137 | //MyMu2.send("Hello.This is mbed. "); |
YSB | 0:7857b4c95c75 | 138 | } |
YSB | 0:7857b4c95c75 | 139 | |
YSB | 0:7857b4c95c75 | 140 | void RHI_Scan(void){ |
YSB | 0:7857b4c95c75 | 141 | servo.TORQUE_ON(1); |
YSB | 0:7857b4c95c75 | 142 | servo.TORQUE_ON(2); |
YSB | 0:7857b4c95c75 | 143 | |
YSB | 0:7857b4c95c75 | 144 | while(1){ |
YSB | 0:7857b4c95c75 | 145 | for(int j=0;j<30;j++){ |
YSB | 0:7857b4c95c75 | 146 | for(int i=0;i<181;i++){ |
YSB | 0:7857b4c95c75 | 147 | servo.Rotate_Servo_Float(1,j*1.0); |
YSB | 0:7857b4c95c75 | 148 | servo.Rotate_Servo_Float(2,90.0-i*1.0); |
YSB | 0:7857b4c95c75 | 149 | wait(0.01); |
YSB | 0:7857b4c95c75 | 150 | } |
YSB | 0:7857b4c95c75 | 151 | for(int i=0;i<181;i++){ |
YSB | 0:7857b4c95c75 | 152 | servo.Rotate_Servo_Float(1,j*1.0); |
YSB | 0:7857b4c95c75 | 153 | servo.Rotate_Servo_Float(2,-90.0+i*1.0); |
YSB | 0:7857b4c95c75 | 154 | wait(0.01); |
YSB | 0:7857b4c95c75 | 155 | } |
YSB | 0:7857b4c95c75 | 156 | wait(0.05); |
YSB | 0:7857b4c95c75 | 157 | } |
YSB | 0:7857b4c95c75 | 158 | } |
YSB | 0:7857b4c95c75 | 159 | } |