Abe Takumi / Mbed 2 deprecated EM_Mission

For Cansat mission mbed

Dependencies:   MU2Class RS405cb mbed myCAN_logger

main.cpp@1:f19466574b75, 2013-07-20 (annotated)

Committer:
YSB
Date:
Sat Jul 20 04:33:14 2013 +0000
Revision:
1:f19466574b75
Parent:
0:7857b4c95c75
Child:
2:5871a792937b
EM

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 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 0:7857b4c95c75 58
YSB 1:f19466574b75 59 while(flightpin ==1){}
YSB 1:f19466574b75 60
YSB 1:f19466574b75 61 MU2_permit = 0;//on
YSB 1:f19466574b75 62 servo.TORQUE_ON(1);
YSB 1:f19466574b75 63 servo.TORQUE_ON(2);
YSB 1:f19466574b75 64 servo.TORQUE_ON(3);
YSB 1:f19466574b75 65 wait(0.3);
YSB 1:f19466574b75 66
YSB 1:f19466574b75 67 MyMu2.send("housyutukennti\r\n");
YSB 1:f19466574b75 68
YSB 1:f19466574b75 69 wait(10);
YSB 1:f19466574b75 70
YSB 1:f19466574b75 71 MyMu2.send("tizyounituita\r\n");
YSB 1:f19466574b75 72
YSB 1:f19466574b75 73
YSB 1:f19466574b75 74 NIKUROMU1_ON
YSB 1:f19466574b75 75 wait(6.0);
YSB 1:f19466574b75 76 NIKUROMU1_OFF
YSB 1:f19466574b75 77
YSB 1:f19466574b75 78 MyMu2.send("tumehiraita\r\n");
YSB 1:f19466574b75 79
YSB 1:f19466574b75 80 wait(10);
YSB 1:f19466574b75 81
YSB 0:7857b4c95c75 82 float theta;
YSB 0:7857b4c95c75 83 theta=-150.0;
YSB 0:7857b4c95c75 84 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 85 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 86 while(1) {
YSB 1:f19466574b75 87 if(can.get_a_z()!=1){
YSB 0:7857b4c95c75 88 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 89 theta += 1.0;
YSB 0:7857b4c95c75 90 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 91 }
YSB 1:f19466574b75 92 else{
YSB 1:f19466574b75 93 servo.TORQUE_ON(3);
YSB 1:f19466574b75 94 break;
YSB 1:f19466574b75 95 }
YSB 0:7857b4c95c75 96 led2 = !led2;
YSB 0:7857b4c95c75 97 }
YSB 0:7857b4c95c75 98
YSB 1:f19466574b75 99 MyMu2.send("uemuitayo\r\n");
YSB 0:7857b4c95c75 100
YSB 1:f19466574b75 101 wait(5.0);
YSB 1:f19466574b75 102 NIKUROMU2_ON
YSB 1:f19466574b75 103 wait(6.0);
YSB 1:f19466574b75 104 NIKUROMU2_OFF
YSB 1:f19466574b75 105
YSB 1:f19466574b75 106 MyMu2.send("tobirahiraita\r\n");
YSB 0:7857b4c95c75 107
YSB 1:f19466574b75 108 wait(5.0);
YSB 1:f19466574b75 109 NIKUROMU3_ON
YSB 1:f19466574b75 110 wait(6.0);
YSB 1:f19466574b75 111 NIKUROMU3_OFF
YSB 0:7857b4c95c75 112
YSB 1:f19466574b75 113 MyMu2.send("anntenatennkai\r\n");
YSB 0:7857b4c95c75 114
YSB 1:f19466574b75 115 MyMu2.send("sukyannhazime\r\n");
YSB 0:7857b4c95c75 116
YSB 1:f19466574b75 117 RHI_Scan();
YSB 0:7857b4c95c75 118 }
YSB 0:7857b4c95c75 119
YSB 0:7857b4c95c75 120 void RHI_Scan(void){
YSB 0:7857b4c95c75 121 servo.TORQUE_ON(1);
YSB 1:f19466574b75 122 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 123
YSB 0:7857b4c95c75 124 while(1){
YSB 0:7857b4c95c75 125 for(int j=0;j<30;j++){
YSB 0:7857b4c95c75 126 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 127 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 128 servo.Rotate_Servo_Float(3,90.0-i*1.0);
YSB 0:7857b4c95c75 129 wait(0.01);
YSB 0:7857b4c95c75 130 }
YSB 0:7857b4c95c75 131 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 132 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 133 servo.Rotate_Servo_Float(3,-90.0+i*1.0);
YSB 0:7857b4c95c75 134 wait(0.01);
YSB 0:7857b4c95c75 135 }
YSB 0:7857b4c95c75 136 wait(0.05);
YSB 0:7857b4c95c75 137 }
YSB 0:7857b4c95c75 138 }
YSB 0:7857b4c95c75 139 }
YSB 1:f19466574b75 140
YSB 1:f19466574b75 141 void data_send(void){
YSB 1:f19466574b75 142 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 143
YSB 1:f19466574b75 144 char mu2data[100];
YSB 1:f19466574b75 145 mu2data[0] = *(can.get_time()+0);
YSB 1:f19466574b75 146 mu2data[1] = *(can.get_time()+1);
YSB 1:f19466574b75 147 mu2data[2] = *(can.get_time()+2);
YSB 1:f19466574b75 148 mu2data[3] = *(can.get_time()+3);
YSB 1:f19466574b75 149 mu2data[4] = *(can.get_time()+4);
YSB 1:f19466574b75 150 mu2data[5] = *(can.get_time()+5);
YSB 1:f19466574b75 151 mu2data[6] = *(can.get_time()+6);
YSB 1:f19466574b75 152 mu2data[7] = *(can.get_time()+7);
YSB 1:f19466574b75 153
YSB 1:f19466574b75 154 mu2data[8]=',';
YSB 1:f19466574b75 155
YSB 1:f19466574b75 156 mu2data[9] = *(can.get_latitude()+0);
YSB 1:f19466574b75 157 mu2data[10] = *(can.get_latitude()+1);
YSB 1:f19466574b75 158 mu2data[11] = *(can.get_latitude()+2);
YSB 1:f19466574b75 159 mu2data[12] = *(can.get_latitude()+3);
YSB 1:f19466574b75 160 mu2data[13] = *(can.get_latitude()+4);
YSB 1:f19466574b75 161 mu2data[14] = *(can.get_latitude()+5);
YSB 1:f19466574b75 162 mu2data[15] = *(can.get_latitude()+6);
YSB 1:f19466574b75 163 mu2data[16] = *(can.get_latitude()+7);
YSB 1:f19466574b75 164 mu2data[17] = *(can.get_latitude()+8);
YSB 1:f19466574b75 165
YSB 1:f19466574b75 166 mu2data[18]=',';
YSB 1:f19466574b75 167
YSB 1:f19466574b75 168 mu2data[19] = *(can.get_longitude()+0);
YSB 1:f19466574b75 169 mu2data[20] = *(can.get_longitude()+1);
YSB 1:f19466574b75 170 mu2data[21] = *(can.get_longitude()+2);
YSB 1:f19466574b75 171 mu2data[22] = *(can.get_longitude()+3);
YSB 1:f19466574b75 172 mu2data[23] = *(can.get_longitude()+4);
YSB 1:f19466574b75 173 mu2data[24] = *(can.get_longitude()+5);
YSB 1:f19466574b75 174 mu2data[25] = *(can.get_longitude()+6);
YSB 1:f19466574b75 175 mu2data[26] = *(can.get_longitude()+7);
YSB 1:f19466574b75 176 mu2data[27] = *(can.get_longitude()+8);
YSB 1:f19466574b75 177 mu2data[28] = *(can.get_longitude()+9);
YSB 1:f19466574b75 178
YSB 1:f19466574b75 179 mu2data[29] = ',';
YSB 1:f19466574b75 180
YSB 1:f19466574b75 181 mu2data[30] = ((can.get_NoS())/10)+0x30;
YSB 1:f19466574b75 182 mu2data[31] = (can.get_NoS()%10)+0x30;
YSB 1:f19466574b75 183
YSB 1:f19466574b75 184 mu2data[32]='\r';
YSB 1:f19466574b75 185 mu2data[33]='\n';
YSB 1:f19466574b75 186 mu2data[34]='\0';
YSB 1:f19466574b75 187
YSB 1:f19466574b75 188 MyMu2.send(mu2data);
YSB 1:f19466574b75 189 }