Abe Takumi / Mbed 2 deprecated EM_Mission

For Cansat mission mbed

Dependencies:   MU2Class RS405cb mbed myCAN_logger

main.cpp@7:6aaf33eac704, 2013-08-16 (annotated)

Committer:
YSB
Date:
Fri Aug 16 08:25:12 2013 +0000
Revision:
7:6aaf33eac704
Parent:
6:c206e90971e1 
20130816ver

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