For Cansat mission mbed
Dependencies: MU2Class RS405cb mbed myCAN_logger
main.cpp@2:5871a792937b, 2013-07-21 (annotated)
- Committer:
- YSB
- Date:
- Sun Jul 21 07:00:11 2013 +0000
- Revision:
- 2:5871a792937b
- Parent:
- 1:f19466574b75
- Child:
- 4:934b39bd5c2c
latest version
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 | 1:f19466574b75 | 4 | #include "myCAN.h" |
YSB | 1:f19466574b75 | 5 | #include "IDDATA.h" |
YSB | 0:7857b4c95c75 | 6 | |
YSB | 0:7857b4c95c75 | 7 | //for debug |
YSB | 0:7857b4c95c75 | 8 | Serial pc(USBTX,USBRX); |
YSB | 0:7857b4c95c75 | 9 | DigitalOut led2(LED2); |
YSB | 0:7857b4c95c75 | 10 | //for communication |
YSB | 1:f19466574b75 | 11 | myCAN can(p30, p29); |
YSB | 0:7857b4c95c75 | 12 | MU2Class MyMu2(p9,p10); |
YSB | 0:7857b4c95c75 | 13 | DigitalOut MU2_permit(p11); |
YSB | 1:f19466574b75 | 14 | Ticker Send_Call; |
YSB | 0:7857b4c95c75 | 15 | //for drive actuator |
YSB | 0:7857b4c95c75 | 16 | RS405cb servo(p13,p14,p18);//TX,RX,PERMIT |
YSB | 0:7857b4c95c75 | 17 | //for fusing PWM |
YSB | 0:7857b4c95c75 | 18 | PwmOut nikuromu1(p22); |
YSB | 0:7857b4c95c75 | 19 | PwmOut nikuromu2(p23); |
YSB | 0:7857b4c95c75 | 20 | PwmOut nikuromu3(p24); |
YSB | 0:7857b4c95c75 | 21 | //for Flight confirmation |
YSB | 0:7857b4c95c75 | 22 | DigitalIn flightpin(p26); |
YSB | 1:f19466574b75 | 23 | |
YSB | 1:f19466574b75 | 24 | //for pc |
YSB | 1:f19466574b75 | 25 | #define PC_BAUD 9600 |
YSB | 1:f19466574b75 | 26 | #define COMMUNICATION_RATE 0.3 |
YSB | 1:f19466574b75 | 27 | void data_send(void); |
YSB | 1:f19466574b75 | 28 | |
YSB | 0:7857b4c95c75 | 29 | //for PWM |
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 | 1:f19466574b75 | 42 | //for pc |
YSB | 1:f19466574b75 | 43 | pc.baud(PC_BAUD); |
YSB | 1:f19466574b75 | 44 | Send_Call.attach(&data_send,COMMUNICATION_RATE); |
YSB | 1:f19466574b75 | 45 | |
YSB | 0:7857b4c95c75 | 46 | //correspondence off |
YSB | 0:7857b4c95c75 | 47 | MU2_permit = 1;//off |
YSB | 0:7857b4c95c75 | 48 | |
YSB | 0:7857b4c95c75 | 49 | //PWM init |
YSB | 0:7857b4c95c75 | 50 | nikuromu1.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 51 | NIKUROMU1_OFF |
YSB | 0:7857b4c95c75 | 52 | nikuromu2.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 53 | NIKUROMU2_OFF |
YSB | 0:7857b4c95c75 | 54 | nikuromu3.period(N_PERIOD); |
YSB | 0:7857b4c95c75 | 55 | NIKUROMU3_OFF |
YSB | 1:f19466574b75 | 56 | |
YSB | 1:f19466574b75 | 57 | //Sequence |
YSB | 2:5871a792937b | 58 | servo.TORQUE_ON(2); |
YSB | 2:5871a792937b | 59 | servo.Rotate_Servo_Float(2,-150.0); |
YSB | 0:7857b4c95c75 | 60 | |
YSB | 1:f19466574b75 | 61 | while(flightpin ==1){} |
YSB | 1:f19466574b75 | 62 | |
YSB | 1:f19466574b75 | 63 | MU2_permit = 0;//on |
YSB | 1:f19466574b75 | 64 | servo.TORQUE_ON(1); |
YSB | 1:f19466574b75 | 65 | servo.TORQUE_ON(2); |
YSB | 1:f19466574b75 | 66 | servo.TORQUE_ON(3); |
YSB | 1:f19466574b75 | 67 | wait(0.3); |
YSB | 1:f19466574b75 | 68 | |
YSB | 1:f19466574b75 | 69 | MyMu2.send("housyutukennti\r\n"); |
YSB | 1:f19466574b75 | 70 | |
YSB | 1:f19466574b75 | 71 | wait(10); |
YSB | 1:f19466574b75 | 72 | |
YSB | 1:f19466574b75 | 73 | MyMu2.send("tizyounituita\r\n"); |
YSB | 1:f19466574b75 | 74 | |
YSB | 1:f19466574b75 | 75 | |
YSB | 1:f19466574b75 | 76 | NIKUROMU1_ON |
YSB | 1:f19466574b75 | 77 | wait(6.0); |
YSB | 1:f19466574b75 | 78 | NIKUROMU1_OFF |
YSB | 1:f19466574b75 | 79 | |
YSB | 1:f19466574b75 | 80 | MyMu2.send("tumehiraita\r\n"); |
YSB | 1:f19466574b75 | 81 | |
YSB | 2:5871a792937b | 82 | wait(30); |
YSB | 1:f19466574b75 | 83 | |
YSB | 0:7857b4c95c75 | 84 | float theta; |
YSB | 0:7857b4c95c75 | 85 | theta=-150.0; |
YSB | 2:5871a792937b | 86 | servo.TORQUE_ON(2); |
YSB | 2:5871a792937b | 87 | servo.Rotate_Servo_Float(2,theta); |
YSB | 0:7857b4c95c75 | 88 | while(1) { |
YSB | 1:f19466574b75 | 89 | if(can.get_a_z()!=1){ |
YSB | 2:5871a792937b | 90 | servo.TORQUE_ON(2); |
YSB | 2:5871a792937b | 91 | theta += 0.1; |
YSB | 2:5871a792937b | 92 | servo.Rotate_Servo_Float(2,theta); |
YSB | 0:7857b4c95c75 | 93 | } |
YSB | 1:f19466574b75 | 94 | else{ |
YSB | 2:5871a792937b | 95 | servo.TORQUE_ON(2); |
YSB | 1:f19466574b75 | 96 | break; |
YSB | 1:f19466574b75 | 97 | } |
YSB | 2:5871a792937b | 98 | wait(0.006); |
YSB | 0:7857b4c95c75 | 99 | led2 = !led2; |
YSB | 0:7857b4c95c75 | 100 | } |
YSB | 0:7857b4c95c75 | 101 | |
YSB | 2:5871a792937b | 102 | wait(20); |
YSB | 2:5871a792937b | 103 | |
YSB | 1:f19466574b75 | 104 | MyMu2.send("uemuitayo\r\n"); |
YSB | 0:7857b4c95c75 | 105 | |
YSB | 1:f19466574b75 | 106 | wait(5.0); |
YSB | 1:f19466574b75 | 107 | NIKUROMU2_ON |
YSB | 1:f19466574b75 | 108 | wait(6.0); |
YSB | 1:f19466574b75 | 109 | NIKUROMU2_OFF |
YSB | 1:f19466574b75 | 110 | |
YSB | 1:f19466574b75 | 111 | MyMu2.send("tobirahiraita\r\n"); |
YSB | 0:7857b4c95c75 | 112 | |
YSB | 2:5871a792937b | 113 | wait(10); |
YSB | 2:5871a792937b | 114 | |
YSB | 1:f19466574b75 | 115 | wait(5.0); |
YSB | 1:f19466574b75 | 116 | NIKUROMU3_ON |
YSB | 1:f19466574b75 | 117 | wait(6.0); |
YSB | 1:f19466574b75 | 118 | NIKUROMU3_OFF |
YSB | 0:7857b4c95c75 | 119 | |
YSB | 1:f19466574b75 | 120 | MyMu2.send("anntenatennkai\r\n"); |
YSB | 0:7857b4c95c75 | 121 | |
YSB | 1:f19466574b75 | 122 | MyMu2.send("sukyannhazime\r\n"); |
YSB | 0:7857b4c95c75 | 123 | |
YSB | 1:f19466574b75 | 124 | RHI_Scan(); |
YSB | 0:7857b4c95c75 | 125 | } |
YSB | 0:7857b4c95c75 | 126 | |
YSB | 0:7857b4c95c75 | 127 | void RHI_Scan(void){ |
YSB | 0:7857b4c95c75 | 128 | servo.TORQUE_ON(1); |
YSB | 1:f19466574b75 | 129 | servo.TORQUE_ON(3); |
YSB | 0:7857b4c95c75 | 130 | |
YSB | 0:7857b4c95c75 | 131 | while(1){ |
YSB | 0:7857b4c95c75 | 132 | for(int j=0;j<30;j++){ |
YSB | 0:7857b4c95c75 | 133 | for(int i=0;i<181;i++){ |
YSB | 0:7857b4c95c75 | 134 | servo.Rotate_Servo_Float(1,j*1.0); |
YSB | 1:f19466574b75 | 135 | servo.Rotate_Servo_Float(3,90.0-i*1.0); |
YSB | 0:7857b4c95c75 | 136 | wait(0.01); |
YSB | 0:7857b4c95c75 | 137 | } |
YSB | 0:7857b4c95c75 | 138 | for(int i=0;i<181;i++){ |
YSB | 0:7857b4c95c75 | 139 | servo.Rotate_Servo_Float(1,j*1.0); |
YSB | 1:f19466574b75 | 140 | servo.Rotate_Servo_Float(3,-90.0+i*1.0); |
YSB | 0:7857b4c95c75 | 141 | wait(0.01); |
YSB | 0:7857b4c95c75 | 142 | } |
YSB | 0:7857b4c95c75 | 143 | wait(0.05); |
YSB | 0:7857b4c95c75 | 144 | } |
YSB | 0:7857b4c95c75 | 145 | } |
YSB | 0:7857b4c95c75 | 146 | } |
YSB | 1:f19466574b75 | 147 | |
YSB | 1:f19466574b75 | 148 | void data_send(void){ |
YSB | 1:f19466574b75 | 149 | pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_a_z()); |
YSB | 1:f19466574b75 | 150 | |
YSB | 1:f19466574b75 | 151 | char mu2data[100]; |
YSB | 1:f19466574b75 | 152 | mu2data[0] = *(can.get_time()+0); |
YSB | 1:f19466574b75 | 153 | mu2data[1] = *(can.get_time()+1); |
YSB | 1:f19466574b75 | 154 | mu2data[2] = *(can.get_time()+2); |
YSB | 1:f19466574b75 | 155 | mu2data[3] = *(can.get_time()+3); |
YSB | 1:f19466574b75 | 156 | mu2data[4] = *(can.get_time()+4); |
YSB | 1:f19466574b75 | 157 | mu2data[5] = *(can.get_time()+5); |
YSB | 1:f19466574b75 | 158 | mu2data[6] = *(can.get_time()+6); |
YSB | 1:f19466574b75 | 159 | mu2data[7] = *(can.get_time()+7); |
YSB | 1:f19466574b75 | 160 | |
YSB | 1:f19466574b75 | 161 | mu2data[8]=','; |
YSB | 1:f19466574b75 | 162 | |
YSB | 1:f19466574b75 | 163 | mu2data[9] = *(can.get_latitude()+0); |
YSB | 1:f19466574b75 | 164 | mu2data[10] = *(can.get_latitude()+1); |
YSB | 1:f19466574b75 | 165 | mu2data[11] = *(can.get_latitude()+2); |
YSB | 1:f19466574b75 | 166 | mu2data[12] = *(can.get_latitude()+3); |
YSB | 1:f19466574b75 | 167 | mu2data[13] = *(can.get_latitude()+4); |
YSB | 1:f19466574b75 | 168 | mu2data[14] = *(can.get_latitude()+5); |
YSB | 1:f19466574b75 | 169 | mu2data[15] = *(can.get_latitude()+6); |
YSB | 1:f19466574b75 | 170 | mu2data[16] = *(can.get_latitude()+7); |
YSB | 1:f19466574b75 | 171 | mu2data[17] = *(can.get_latitude()+8); |
YSB | 1:f19466574b75 | 172 | |
YSB | 1:f19466574b75 | 173 | mu2data[18]=','; |
YSB | 1:f19466574b75 | 174 | |
YSB | 1:f19466574b75 | 175 | mu2data[19] = *(can.get_longitude()+0); |
YSB | 1:f19466574b75 | 176 | mu2data[20] = *(can.get_longitude()+1); |
YSB | 1:f19466574b75 | 177 | mu2data[21] = *(can.get_longitude()+2); |
YSB | 1:f19466574b75 | 178 | mu2data[22] = *(can.get_longitude()+3); |
YSB | 1:f19466574b75 | 179 | mu2data[23] = *(can.get_longitude()+4); |
YSB | 1:f19466574b75 | 180 | mu2data[24] = *(can.get_longitude()+5); |
YSB | 1:f19466574b75 | 181 | mu2data[25] = *(can.get_longitude()+6); |
YSB | 1:f19466574b75 | 182 | mu2data[26] = *(can.get_longitude()+7); |
YSB | 1:f19466574b75 | 183 | mu2data[27] = *(can.get_longitude()+8); |
YSB | 1:f19466574b75 | 184 | mu2data[28] = *(can.get_longitude()+9); |
YSB | 1:f19466574b75 | 185 | |
YSB | 1:f19466574b75 | 186 | mu2data[29] = ','; |
YSB | 1:f19466574b75 | 187 | |
YSB | 1:f19466574b75 | 188 | mu2data[30] = ((can.get_NoS())/10)+0x30; |
YSB | 1:f19466574b75 | 189 | mu2data[31] = (can.get_NoS()%10)+0x30; |
YSB | 1:f19466574b75 | 190 | |
YSB | 1:f19466574b75 | 191 | mu2data[32]='\r'; |
YSB | 1:f19466574b75 | 192 | mu2data[33]='\n'; |
YSB | 1:f19466574b75 | 193 | mu2data[34]='\0'; |
YSB | 1:f19466574b75 | 194 | |
YSB | 1:f19466574b75 | 195 | MyMu2.send(mu2data); |
YSB | 1:f19466574b75 | 196 | } |