Abe Takumi / Mbed 2 deprecated EM_Mission

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?

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 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 }