Taiyo Koga / Mbed 2 deprecated teamA_saramawashi

Published late sorry

Dependencies:   mbed PS_PAD

main.cpp@2:ea130e3087f8, 2020-09-09 (annotated)

Committer:
koga641028
Date:
Wed Sep 09 01:01:52 2020 +0000
Revision:
2:ea130e3087f8
Parent:
1:ef44f09e471e 
unchi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koga641028 0:50825421a4ce 1 /*
koga641028 0:50825421a4ce 2 * ======================
koga641028 0:50825421a4ce 3 * VS-C3/VS-RCV3 PIN
koga641028 0:50825421a4ce 4 * ======================
koga641028 0:50825421a4ce 5 * 1:NC 2:NC
koga641028 0:50825421a4ce 6 * 3:DAT 4:CMD
koga641028 0:50825421a4ce 7 * 5:SEL 6:CLK
koga641028 0:50825421a4ce 8 * 7:+5~7V 8:NC
koga641028 0:50825421a4ce 9 * 9:+3V 10:GND
koga641028 0:50825421a4ce 10 *
koga641028 0:50825421a4ce 11 */
koga641028 0:50825421a4ce 12
koga641028 0:50825421a4ce 13 #include "mbed.h"
koga641028 0:50825421a4ce 14 #include "PS_PAD.h"
koga641028 2:ea130e3087f8 15
koga641028 0:50825421a4ce 16 class Motor{
koga641028 0:50825421a4ce 17 PwmOut a, b;
koga641028 0:50825421a4ce 18 public:
koga641028 0:50825421a4ce 19 Motor(PinName A_term, PinName B_term) : a(A_term), b(B_term) {
koga641028 1:ef44f09e471e 20 a.period(0.0002);
koga641028 1:ef44f09e471e 21 b.period(0.0002);
koga641028 0:50825421a4ce 22 }
koga641028 0:50825421a4ce 23
koga641028 0:50825421a4ce 24 Motor& operator=(double duty) {
koga641028 0:50825421a4ce 25 if(duty > 0){
koga641028 0:50825421a4ce 26 a = duty < 1 ? duty : 1;
koga641028 0:50825421a4ce 27 b = 0;
koga641028 0:50825421a4ce 28 }else {
koga641028 0:50825421a4ce 29 a = 0;
koga641028 1:ef44f09e471e 30 b = duty > -1 ? -duty : 1;
koga641028 0:50825421a4ce 31 }
koga641028 0:50825421a4ce 32 return *this;
koga641028 0:50825421a4ce 33 }
koga641028 1:ef44f09e471e 34 operator double() {
koga641028 1:ef44f09e471e 35 if(a > 0)
koga641028 1:ef44f09e471e 36 return (double)a;
koga641028 1:ef44f09e471e 37 else
koga641028 1:ef44f09e471e 38 return -(double)b;
koga641028 1:ef44f09e471e 39 }
koga641028 0:50825421a4ce 40 };
koga641028 1:ef44f09e471e 41
koga641028 1:ef44f09e471e 42 DigitalOut Led[] = {PB_7, PB_14, PB_0, };
koga641028 0:50825421a4ce 43 PS_PAD vsc3(PA_7, PA_6, PA_5, PA_4); //MOSI, MISO, SCK, SS
koga641028 1:ef44f09e471e 44
koga641028 0:50825421a4ce 45 DigitalIn lim[7] = {PD_2, PD_3, PD_4, PD_5, PD_6, PD_7, PG_9};
koga641028 0:50825421a4ce 46 Motor rmotor(PE_9, PE_11);
koga641028 0:50825421a4ce 47 Motor lmotor(PE_13, PE_14);
koga641028 1:ef44f09e471e 48 Motor rsmotor(PA_0, PA_1);
koga641028 1:ef44f09e471e 49 Motor lsmotor(PA_2, PA_3);
koga641028 1:ef44f09e471e 50 Motor prmotor(PC_8, PC_9);
koga641028 1:ef44f09e471e 51 Motor plmotor(PC_6, PC_7);
koga641028 1:ef44f09e471e 52 PwmOut erservo(PD_14);
koga641028 1:ef44f09e471e 53 PwmOut elservo(PD_15);
koga641028 0:50825421a4ce 54
koga641028 1:ef44f09e471e 55
koga641028 0:50825421a4ce 56 int main() {
koga641028 0:50825421a4ce 57 vsc3.init();
koga641028 2:ea130e3087f8 58 int n = 0;
koga641028 1:ef44f09e471e 59 double movey = 0, turn = 0, speed1 = 0.5, speed2 = 0.5;
koga641028 2:ea130e3087f8 60 double speed3 = 0, speed4 = 0 ;
koga641028 1:ef44f09e471e 61 bool selcli = false, stacli = false;
koga641028 0:50825421a4ce 62 printf("START\n");
koga641028 1:ef44f09e471e 63 elservo.period(0.020);
koga641028 1:ef44f09e471e 64 erservo.period(0.020);
koga641028 0:50825421a4ce 65
koga641028 0:50825421a4ce 66 while(1) {
koga641028 0:50825421a4ce 67 vsc3.poll();
koga641028 1:ef44f09e471e 68 if (vsc3.read(PS_PAD::PAD_BOTTOM)){
koga641028 0:50825421a4ce 69 movey = -1;
koga641028 1:ef44f09e471e 70 }else if (vsc3.read(PS_PAD::PAD_TOP)){
koga641028 0:50825421a4ce 71 movey = 1;
koga641028 1:ef44f09e471e 72 /*}else if (abs(vsc3.read(PS_PAD::ANALOG_LY)) > 10){
koga641028 1:ef44f09e471e 73 movey = (vsc3.read(PS_PAD::ANALOG_LY)) / 128.0;*/
koga641028 0:50825421a4ce 74 }else{
koga641028 0:50825421a4ce 75 movey = 0;
koga641028 0:50825421a4ce 76 }
koga641028 0:50825421a4ce 77
koga641028 0:50825421a4ce 78 if (vsc3.read(PS_PAD::PAD_RIGHT)){
koga641028 2:ea130e3087f8 79 turn = 0.8;
koga641028 1:ef44f09e471e 80 }else if (vsc3.read(PS_PAD::PAD_LEFT)){
koga641028 2:ea130e3087f8 81 turn = -0.8;
koga641028 1:ef44f09e471e 82 /*}else if (abs(vsc3.read(PS_PAD::ANALOG_LX)) > 10){
koga641028 1:ef44f09e471e 83 turn = (vsc3.read(PS_PAD::ANALOG_LX)) / 128.0;*/
koga641028 0:50825421a4ce 84 }else{
koga641028 0:50825421a4ce 85 turn = 0;
koga641028 0:50825421a4ce 86 }
koga641028 0:50825421a4ce 87
koga641028 0:50825421a4ce 88 rmotor = -movey+turn; //move
koga641028 0:50825421a4ce 89 lmotor = -movey-turn;
koga641028 0:50825421a4ce 90
koga641028 1:ef44f09e471e 91 /*saramawashi*/
koga641028 2:ea130e3087f8 92
koga641028 1:ef44f09e471e 93
koga641028 1:ef44f09e471e 94 if (vsc3.read(PS_PAD::PAD_R2) /*&& lim[0] == 0*/){ //limitswitch
koga641028 1:ef44f09e471e 95 rsmotor = speed1;
koga641028 2:ea130e3087f8 96 }else if (vsc3.read(PS_PAD::PAD_R1) /*&& lim[0] == 0*/){ //limitswitch
koga641028 2:ea130e3087f8 97 rsmotor = -speed1;
koga641028 1:ef44f09e471e 98 }else {
koga641028 1:ef44f09e471e 99 rsmotor = 0;
koga641028 1:ef44f09e471e 100 }
koga641028 1:ef44f09e471e 101 if (vsc3.read(PS_PAD::PAD_L2) /*&& lim[0] == 0*/){ //limitswitch
koga641028 1:ef44f09e471e 102 lsmotor = speed2;
koga641028 2:ea130e3087f8 103 }else if (vsc3.read(PS_PAD::PAD_L1) /*&& lim[0] == 0*/){ //limitswitch
koga641028 2:ea130e3087f8 104 lsmotor = -speed2;
koga641028 2:ea130e3087f8 105
koga641028 1:ef44f09e471e 106 }else {
koga641028 1:ef44f09e471e 107 lsmotor = 0;
koga641028 1:ef44f09e471e 108 }
koga641028 1:ef44f09e471e 109
koga641028 1:ef44f09e471e 110 if (vsc3.read(PS_PAD::PAD_SELECT) && !selcli){
koga641028 2:ea130e3087f8 111 n++;
koga641028 2:ea130e3087f8 112 speed1 = 0.05*n*n;
koga641028 2:ea130e3087f8 113 speed2 = 0.05*n*n;
koga641028 1:ef44f09e471e 114 if (speed1 < 0)
koga641028 1:ef44f09e471e 115 speed1 = 0;
koga641028 1:ef44f09e471e 116 if (speed2 < 0)
koga641028 1:ef44f09e471e 117 speed2 = 0;
koga641028 2:ea130e3087f8 118 if ( n < 0)
koga641028 2:ea130e3087f8 119 n = 0;
koga641028 1:ef44f09e471e 120 selcli = true;
koga641028 1:ef44f09e471e 121 }
koga641028 1:ef44f09e471e 122 if (!vsc3.read(PS_PAD::PAD_SELECT)){
koga641028 1:ef44f09e471e 123 selcli = false;
koga641028 1:ef44f09e471e 124 }
koga641028 1:ef44f09e471e 125
koga641028 1:ef44f09e471e 126 if (vsc3.read(PS_PAD::PAD_START) && !stacli){
koga641028 2:ea130e3087f8 127 n--;
koga641028 2:ea130e3087f8 128 speed1 = 0.05*n*n;
koga641028 2:ea130e3087f8 129 speed2 = 0.05*n*n;
koga641028 1:ef44f09e471e 130 if (speed1 > 1)
koga641028 1:ef44f09e471e 131 speed1 = 1;
koga641028 1:ef44f09e471e 132 if (speed2 > 1)
koga641028 1:ef44f09e471e 133 speed2 = 1;
koga641028 2:ea130e3087f8 134 if ( n > 1)
koga641028 2:ea130e3087f8 135 n = 1;
koga641028 1:ef44f09e471e 136 stacli = true;
koga641028 1:ef44f09e471e 137 }
koga641028 1:ef44f09e471e 138 if (!vsc3.read(PS_PAD::PAD_START)){
koga641028 1:ef44f09e471e 139 stacli = false;
koga641028 0:50825421a4ce 140 }
koga641028 0:50825421a4ce 141
koga641028 1:ef44f09e471e 142 if (vsc3.read(PS_PAD::ANALOG_RY) > 100){
koga641028 1:ef44f09e471e 143 speed3 += 0.03;
koga641028 2:ea130e3087f8 144 speed1 += 0.05;
koga641028 1:ef44f09e471e 145 if(speed3 > 0.3)
koga641028 1:ef44f09e471e 146 speed3 = 0.3;
koga641028 1:ef44f09e471e 147 }else if (vsc3.read(PS_PAD::ANALOG_RY) < -100){
koga641028 1:ef44f09e471e 148 speed3 -= 0.03;
koga641028 2:ea130e3087f8 149 speed1 -= 0.05;
koga641028 1:ef44f09e471e 150 if(speed3 < -0.3)
koga641028 1:ef44f09e471e 151 speed3 = -0.3;
koga641028 1:ef44f09e471e 152 }else {
koga641028 1:ef44f09e471e 153 if(speed3 < 0){
koga641028 1:ef44f09e471e 154 speed3 += 0.03;
koga641028 1:ef44f09e471e 155 if(speed3 > 0)
koga641028 1:ef44f09e471e 156 speed3 = 0;
koga641028 1:ef44f09e471e 157 }else if(speed3 > 0){
koga641028 1:ef44f09e471e 158 speed3 -= 0.03;
koga641028 1:ef44f09e471e 159 if(speed3 < 0)
koga641028 1:ef44f09e471e 160 speed3 = 0;
koga641028 1:ef44f09e471e 161 }
koga641028 1:ef44f09e471e 162 }
koga641028 1:ef44f09e471e 163
koga641028 1:ef44f09e471e 164 prmotor = speed3;
koga641028 0:50825421a4ce 165
koga641028 1:ef44f09e471e 166 if (vsc3.read(PS_PAD::ANALOG_LY) > 100){
koga641028 1:ef44f09e471e 167 speed4 += 0.03;
koga641028 2:ea130e3087f8 168 speed2 += 0.05;
koga641028 1:ef44f09e471e 169 if(speed4 > 0.3)
koga641028 1:ef44f09e471e 170 speed4 = 0.3;
koga641028 1:ef44f09e471e 171 }else if (vsc3.read(PS_PAD::ANALOG_LY) < -100){
koga641028 1:ef44f09e471e 172 speed4 -= 0.03;
koga641028 2:ea130e3087f8 173 speed2 -= 0.05;
koga641028 1:ef44f09e471e 174 if(speed4 < -0.3)
koga641028 1:ef44f09e471e 175 speed4 = -0.3;
koga641028 1:ef44f09e471e 176 }else {
koga641028 1:ef44f09e471e 177 if(speed4 < 0){
koga641028 1:ef44f09e471e 178 speed4 += 0.03;
koga641028 1:ef44f09e471e 179 if(speed4 > 0)
koga641028 1:ef44f09e471e 180 speed4 = 0;
koga641028 1:ef44f09e471e 181 }else if(speed4 > 0){
koga641028 1:ef44f09e471e 182 speed4 -= 0.03;
koga641028 1:ef44f09e471e 183 if(speed4 < 0)
koga641028 1:ef44f09e471e 184 speed4 = 0;
koga641028 1:ef44f09e471e 185 }
koga641028 0:50825421a4ce 186 }
koga641028 1:ef44f09e471e 187
koga641028 1:ef44f09e471e 188 plmotor = speed4;
koga641028 1:ef44f09e471e 189
koga641028 1:ef44f09e471e 190 //eye move
koga641028 2:ea130e3087f8 191 if (vsc3.read(PS_PAD::PAD_SQUARE)){
koga641028 1:ef44f09e471e 192 elservo.pulsewidth(0.0017);
koga641028 1:ef44f09e471e 193 erservo.pulsewidth(0.0012);
koga641028 2:ea130e3087f8 194 }else if (vsc3.read(PS_PAD::PAD_CIRCLE)){
koga641028 1:ef44f09e471e 195 elservo.pulsewidth(0.0012);
koga641028 1:ef44f09e471e 196 erservo.pulsewidth(0.0017);
koga641028 2:ea130e3087f8 197 }else if (vsc3.read(PS_PAD::PAD_TRIANGLE)){
koga641028 1:ef44f09e471e 198 elservo.pulsewidth(0.0015);
koga641028 1:ef44f09e471e 199 erservo.pulsewidth(0.0015);
koga641028 1:ef44f09e471e 200 }
koga641028 1:ef44f09e471e 201
koga641028 0:50825421a4ce 202
koga641028 1:ef44f09e471e 203 /*if (vsc3.read(PS_PAD::PAD_L1) && lim[6] == 0){
koga641028 0:50825421a4ce 204 tlmotor = 1;
koga641028 1:ef44f09e471e 205 }else if (vsc3.read(PS_PAD::PAD_L2) && lim[7] == 0){
koga641028 0:50825421a4ce 206 tlmotor = -1;
koga641028 0:50825421a4ce 207 }else{
koga641028 0:50825421a4ce 208 tlmotor = 0;
koga641028 1:ef44f09e471e 209 }*/
koga641028 0:50825421a4ce 210
koga641028 0:50825421a4ce 211 wait(0.05);
koga641028 0:50825421a4ce 212 }
koga641028 0:50825421a4ce 213 }