Abe Takumi / Mbed 2 deprecated EM_Mission

For Cansat mission mbed

Dependencies:   MU2Class RS405cb mbed myCAN_logger

main.cpp@5:e85af85e4985, 2013-08-03 (annotated)

Committer:
YSB
Date:
Sat Aug 03 09:13:53 2013 +0000
Revision:
5:e85af85e4985
Parent:
4:934b39bd5c2c
Child:
6:c206e90971e1
aaaa

Who changed what in which revision?

UserRevisionLine numberNew 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 5:e85af85e4985 23 //for landing
YSB 5:e85af85e4985 24 Timeout landing_timeout;
YSB 1:f19466574b75 25
YSB 1:f19466574b75 26 //for pc
YSB 1:f19466574b75 27 #define PC_BAUD 9600
YSB 1:f19466574b75 28 #define COMMUNICATION_RATE 0.3
YSB 1:f19466574b75 29 void data_send(void);
YSB 1:f19466574b75 30
YSB 0:7857b4c95c75 31 //for PWM
YSB 0:7857b4c95c75 32 #define N_PERIOD 0.01
YSB 0:7857b4c95c75 33 #define NIKUROMU1_OFF nikuromu1.pulsewidth(0.0);
YSB 0:7857b4c95c75 34 #define NIKUROMU2_OFF nikuromu2.pulsewidth(0.0);
YSB 0:7857b4c95c75 35 #define NIKUROMU3_OFF nikuromu3.pulsewidth(0.0);
YSB 0:7857b4c95c75 36 #define NIKUROMU1_ON nikuromu1.pulsewidth(0.003);
YSB 0:7857b4c95c75 37 #define NIKUROMU2_ON nikuromu2.pulsewidth(0.003);
YSB 0:7857b4c95c75 38 #define NIKUROMU3_ON nikuromu3.pulsewidth(0.003);
YSB 0:7857b4c95c75 39
YSB 0:7857b4c95c75 40 //for servo
YSB 0:7857b4c95c75 41 void RHI_Scan(void);
YSB 5:e85af85e4985 42
YSB 5:e85af85e4985 43 //for landing
YSB 5:e85af85e4985 44 void landing(void);
YSB 5:e85af85e4985 45 char landing_flg=0;
YSB 5:e85af85e4985 46
YSB 5:e85af85e4985 47 void landing(void){
YSB 5:e85af85e4985 48 landing_flg=1;
YSB 5:e85af85e4985 49 MyMu2.send("landing_timeout\r\n");
YSB 5:e85af85e4985 50 }
YSB 0:7857b4c95c75 51
YSB 0:7857b4c95c75 52 int main() {
YSB 5:e85af85e4985 53 //////////Init///////////////////////////////////////////////////////////////////////////////
YSB 1:f19466574b75 54 //for pc
YSB 1:f19466574b75 55 pc.baud(PC_BAUD);
YSB 1:f19466574b75 56 Send_Call.attach(&data_send,COMMUNICATION_RATE);
YSB 1:f19466574b75 57
YSB 0:7857b4c95c75 58 //correspondence off
YSB 0:7857b4c95c75 59 MU2_permit = 1;//off
YSB 0:7857b4c95c75 60
YSB 0:7857b4c95c75 61 //PWM init
YSB 0:7857b4c95c75 62 nikuromu1.period(N_PERIOD);
YSB 0:7857b4c95c75 63 NIKUROMU1_OFF
YSB 0:7857b4c95c75 64 nikuromu2.period(N_PERIOD);
YSB 0:7857b4c95c75 65 NIKUROMU2_OFF
YSB 0:7857b4c95c75 66 nikuromu3.period(N_PERIOD);
YSB 0:7857b4c95c75 67 NIKUROMU3_OFF
YSB 1:f19466574b75 68
YSB 5:e85af85e4985 69 //servo init
YSB 4:934b39bd5c2c 70 servo.TORQUE_ON(2); //for rotation
YSB 2:5871a792937b 71 servo.Rotate_Servo_Float(2,-150.0);
YSB 5:e85af85e4985 72
YSB 5:e85af85e4985 73 //for debug test
YSB 5:e85af85e4985 74 //servo.Rotate_Servo_Float_Test(1);
YSB 5:e85af85e4985 75 //servo.ID_CHANGE(1,2);
YSB 5:e85af85e4985 76
YSB 5:e85af85e4985 77 ///////Sequence////////////////////////////////////////////////////////////////////////
YSB 1:f19466574b75 78
YSB 5:e85af85e4985 79 //0.MU2 check
YSB 1:f19466574b75 80 MU2_permit = 0;//on
YSB 5:e85af85e4985 81 wait(0.1);//wait until mu2 will be steady
YSB 5:e85af85e4985 82 MyMu2.send("we will start cansat mission soon.\r\n");
YSB 5:e85af85e4985 83 MyMu2.send("this message is MU2 confirmation.\r\n");
YSB 5:e85af85e4985 84 MyMu2.send("we will turn off MU2 for regulation afetr 3 sec.\r\n");
YSB 5:e85af85e4985 85 MyMu2.send("3\r\n");
YSB 5:e85af85e4985 86 wait(1);
YSB 5:e85af85e4985 87 MyMu2.send("2\r\n");
YSB 5:e85af85e4985 88 wait(1);
YSB 5:e85af85e4985 89 MyMu2.send("1\r\n");
YSB 5:e85af85e4985 90 wait(1);
YSB 5:e85af85e4985 91 MyMu2.send("0 please check MU2 is off \r\n");
YSB 5:e85af85e4985 92 MU2_permit = 1;//off
YSB 5:e85af85e4985 93
YSB 5:e85af85e4985 94 //1.freeze sequence while flightpin are fixed
YSB 5:e85af85e4985 95 while(flightpin ==1){}
YSB 5:e85af85e4985 96 //after emission
YSB 5:e85af85e4985 97 MU2_permit = 0;//on
YSB 5:e85af85e4985 98 wait(0.1);//wait until mu2 will be steady
YSB 5:e85af85e4985 99 servo.TORQUE_ON(1);
YSB 5:e85af85e4985 100 servo.TORQUE_ON(2);
YSB 5:e85af85e4985 101 servo.TORQUE_ON(3);
YSB 5:e85af85e4985 102 MyMu2.send("emission finished!\r\n");
YSB 1:f19466574b75 103
YSB 5:e85af85e4985 104 //2.check landing
YSB 5:e85af85e4985 105 int roll_data[10];
YSB 5:e85af85e4985 106 int roll_average;
YSB 5:e85af85e4985 107 int roll_breakup;
YSB 5:e85af85e4985 108 landing_timeout.attach(&landing, 30.0);
YSB 5:e85af85e4985 109 while(landing_flg==0){
YSB 5:e85af85e4985 110 MyMu2.send("falling...\r\n");
YSB 5:e85af85e4985 111 roll_average=0;
YSB 5:e85af85e4985 112 roll_breakup=0;
YSB 5:e85af85e4985 113 for(int i=0;i<10;i++){
YSB 5:e85af85e4985 114 roll_data[i]=can.get_roll();
YSB 5:e85af85e4985 115 roll_average += roll_data[i];
YSB 5:e85af85e4985 116 wait(0.5);
YSB 5:e85af85e4985 117 }
YSB 5:e85af85e4985 118 roll_average = roll_average/10;
YSB 5:e85af85e4985 119 for(int i=0;i<10;i++){
YSB 5:e85af85e4985 120 roll_breakup += (roll_data[i]-roll_average)*(roll_data[i]-roll_average);
YSB 5:e85af85e4985 121 }
YSB 5:e85af85e4985 122 if(roll_breakup < 10){
YSB 5:e85af85e4985 123 landing_flg=1;
YSB 5:e85af85e4985 124 }
YSB 5:e85af85e4985 125 }
YSB 1:f19466574b75 126
YSB 5:e85af85e4985 127 wait(5);//rest
YSB 5:e85af85e4985 128
YSB 5:e85af85e4985 129 //3.fix mechanism start
YSB 5:e85af85e4985 130 MyMu2.send("start fix mechanism\r\n");
YSB 1:f19466574b75 131 NIKUROMU1_ON
YSB 1:f19466574b75 132 wait(6.0);
YSB 1:f19466574b75 133 NIKUROMU1_OFF
YSB 5:e85af85e4985 134 MyMu2.send("finish fix mechanism\r\n");
YSB 5:e85af85e4985 135
YSB 5:e85af85e4985 136 wait(5);//rest
YSB 1:f19466574b75 137
YSB 5:e85af85e4985 138 //4.rotation mechanism start
YSB 5:e85af85e4985 139 MyMu2.send("start rotation mechanism\r\n");
YSB 2:5871a792937b 140 servo.TORQUE_ON(2);
YSB 4:934b39bd5c2c 141 servo.Rotate_Servo_Float(2,-150.0);
YSB 5:e85af85e4985 142 servo.Rotate_Servo_Float(2,-150.0 + 360.0 - can.get_roll() );
YSB 5:e85af85e4985 143 MyMu2.send("finish rotation mechanism\r\n");
YSB 2:5871a792937b 144
YSB 5:e85af85e4985 145 wait(5);//rest
YSB 0:7857b4c95c75 146
YSB 5:e85af85e4985 147 //5.open door
YSB 5:e85af85e4985 148 MyMu2.send("start opening door\r\n");
YSB 1:f19466574b75 149 NIKUROMU2_ON
YSB 1:f19466574b75 150 wait(6.0);
YSB 1:f19466574b75 151 NIKUROMU2_OFF
YSB 5:e85af85e4985 152 MyMu2.send("finish opening door\r\n");
YSB 5:e85af85e4985 153
YSB 5:e85af85e4985 154 wait(5);//rest
YSB 0:7857b4c95c75 155
YSB 5:e85af85e4985 156 //6.develope parabolic antenna
YSB 5:e85af85e4985 157 MyMu2.send("start developing parabolic antenna\r\n");
YSB 1:f19466574b75 158 NIKUROMU3_ON
YSB 1:f19466574b75 159 wait(6.0);
YSB 1:f19466574b75 160 NIKUROMU3_OFF
YSB 5:e85af85e4985 161 MyMu2.send("finish developing parabolic antenna\r\n");
YSB 5:e85af85e4985 162
YSB 5:e85af85e4985 163 wait(5);//rest
YSB 0:7857b4c95c75 164
YSB 5:e85af85e4985 165 //7.start RHI scaning
YSB 5:e85af85e4985 166 MyMu2.send("start RHI scaning\r\n");
YSB 1:f19466574b75 167 RHI_Scan();
YSB 5:e85af85e4985 168
YSB 5:e85af85e4985 169 //8.end of sequence
YSB 5:e85af85e4985 170 MyMu2.send("END!\r\n");
YSB 0:7857b4c95c75 171 }
YSB 0:7857b4c95c75 172
YSB 0:7857b4c95c75 173 void RHI_Scan(void){
YSB 0:7857b4c95c75 174 servo.TORQUE_ON(1);
YSB 1:f19466574b75 175 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 176
YSB 0:7857b4c95c75 177 while(1){
YSB 4:934b39bd5c2c 178 for(int j=0;j<100;j++){
YSB 0:7857b4c95c75 179 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 180 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 181 servo.Rotate_Servo_Float(3,90.0-i*1.0);
YSB 0:7857b4c95c75 182 wait(0.01);
YSB 0:7857b4c95c75 183 }
YSB 0:7857b4c95c75 184 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 185 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 186 servo.Rotate_Servo_Float(3,-90.0+i*1.0);
YSB 0:7857b4c95c75 187 wait(0.01);
YSB 0:7857b4c95c75 188 }
YSB 0:7857b4c95c75 189 wait(0.05);
YSB 0:7857b4c95c75 190 }
YSB 0:7857b4c95c75 191 }
YSB 0:7857b4c95c75 192 }
YSB 1:f19466574b75 193
YSB 1:f19466574b75 194 void data_send(void){
YSB 4:934b39bd5c2c 195 pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_roll());
YSB 1:f19466574b75 196
YSB 1:f19466574b75 197 char mu2data[100];
YSB 1:f19466574b75 198 mu2data[0] = *(can.get_time()+0);
YSB 1:f19466574b75 199 mu2data[1] = *(can.get_time()+1);
YSB 1:f19466574b75 200 mu2data[2] = *(can.get_time()+2);
YSB 1:f19466574b75 201 mu2data[3] = *(can.get_time()+3);
YSB 1:f19466574b75 202 mu2data[4] = *(can.get_time()+4);
YSB 1:f19466574b75 203 mu2data[5] = *(can.get_time()+5);
YSB 1:f19466574b75 204 mu2data[6] = *(can.get_time()+6);
YSB 1:f19466574b75 205 mu2data[7] = *(can.get_time()+7);
YSB 1:f19466574b75 206
YSB 1:f19466574b75 207 mu2data[8]=',';
YSB 1:f19466574b75 208
YSB 1:f19466574b75 209 mu2data[9] = *(can.get_latitude()+0);
YSB 1:f19466574b75 210 mu2data[10] = *(can.get_latitude()+1);
YSB 1:f19466574b75 211 mu2data[11] = *(can.get_latitude()+2);
YSB 1:f19466574b75 212 mu2data[12] = *(can.get_latitude()+3);
YSB 1:f19466574b75 213 mu2data[13] = *(can.get_latitude()+4);
YSB 1:f19466574b75 214 mu2data[14] = *(can.get_latitude()+5);
YSB 1:f19466574b75 215 mu2data[15] = *(can.get_latitude()+6);
YSB 1:f19466574b75 216 mu2data[16] = *(can.get_latitude()+7);
YSB 1:f19466574b75 217 mu2data[17] = *(can.get_latitude()+8);
YSB 1:f19466574b75 218
YSB 1:f19466574b75 219 mu2data[18]=',';
YSB 1:f19466574b75 220
YSB 1:f19466574b75 221 mu2data[19] = *(can.get_longitude()+0);
YSB 1:f19466574b75 222 mu2data[20] = *(can.get_longitude()+1);
YSB 1:f19466574b75 223 mu2data[21] = *(can.get_longitude()+2);
YSB 1:f19466574b75 224 mu2data[22] = *(can.get_longitude()+3);
YSB 1:f19466574b75 225 mu2data[23] = *(can.get_longitude()+4);
YSB 1:f19466574b75 226 mu2data[24] = *(can.get_longitude()+5);
YSB 1:f19466574b75 227 mu2data[25] = *(can.get_longitude()+6);
YSB 1:f19466574b75 228 mu2data[26] = *(can.get_longitude()+7);
YSB 1:f19466574b75 229 mu2data[27] = *(can.get_longitude()+8);
YSB 1:f19466574b75 230 mu2data[28] = *(can.get_longitude()+9);
YSB 1:f19466574b75 231
YSB 1:f19466574b75 232 mu2data[29] = ',';
YSB 1:f19466574b75 233
YSB 1:f19466574b75 234 mu2data[30] = ((can.get_NoS())/10)+0x30;
YSB 1:f19466574b75 235 mu2data[31] = (can.get_NoS()%10)+0x30;
YSB 5:e85af85e4985 236
YSB 5:e85af85e4985 237 mu2data[32] = ',';
YSB 5:e85af85e4985 238
YSB 5:e85af85e4985 239 mu2data[33]= (can.get_roll()/100)+0x30;
YSB 5:e85af85e4985 240 mu2data[34]=((can.get_roll()%100 - can.get_roll()%10)/10)+0x30;
YSB 5:e85af85e4985 241 mu2data[35]=(can.get_roll()%10)+0x30;;
YSB 1:f19466574b75 242
YSB 5:e85af85e4985 243 mu2data[36]='\r';
YSB 5:e85af85e4985 244 mu2data[37]='\n';
YSB 5:e85af85e4985 245 mu2data[38]='\0';
YSB 1:f19466574b75 246
YSB 1:f19466574b75 247 MyMu2.send(mu2data);
YSB 1:f19466574b75 248 }